home *** CD-ROM | disk | FTP | other *** search
/ AI Game Programming Wisdom / AIGameProgrammingWisdom.iso / SourceCode / 09 Racing and Sports AI / 04 Adzima / aiNavigation.cpp < prev    next >
Encoding:
C/C++ Source or Header  |  2001-08-19  |  83.5 KB  |  2,891 lines
  1. #include "aiNavigation.h"
  2. #include "aiMap.h"
  3. #include "aiObstacle.h"
  4.  
  5. static int _nMaxTrys=10;
  6. static int _nMaxRouteSplits=5;
  7. static float _fFriction=1.2f;
  8. static float _fTurnThreshold=.7f;
  9. static float _fMaxStraightAngle=.2f;
  10.  
  11. extern aiMap AIMAP;
  12.  
  13. inline short aiNavigation::CheckPt(int nIdx)
  14. {
  15.     if(nIdx<0 || nIdx>=m_nNumWayPts)
  16.     {
  17.         return -1;
  18.     }
  19.     return m_panWayPts[nIdx];
  20. }
  21.  
  22. aiNavigation::aiNavigation()
  23. {
  24. }
  25.  
  26. aiRouteNode::aiRouteNode() {
  27.     Reset();
  28. }
  29.  
  30. void aiRouteNode::Reset() {
  31.     m_pObstacle=0;
  32.     Vtx.Zero();
  33.     m_fCumAngle=0.0f;
  34.     m_fCumDist=0.0f;
  35.     m_nSRd=0;
  36.     m_nSVIdx=0;
  37.     m_nType=0;
  38.     m_nTurnId=0;
  39.     m_nObsType=0;
  40.     m_nOnRoad=0;    // 0: Off the road, 1: On the road, 2: On the sidewalk
  41. }
  42.  
  43. aiNavigation::~aiNavigation()
  44. {
  45. }
  46.  
  47. void aiNavigation::Init(int nId,char *pGeoFile) 
  48. {
  49.     m_Car.Init(nId,pGeoFile); 
  50.  
  51.     Vector3 Min=m_Car.GetBoxMin();
  52.     Vector3 Max=m_Car.GetBoxMax();
  53.     m_fFrontBumperDistance=-Min.z;
  54.     m_fBackBumperDistance=Max.z;
  55.     m_fLSideDist=-Min.x;
  56.     m_fRSideDist=Max.x;
  57.  
  58.     Reset();
  59. }
  60.  
  61. void aiNavigation::Reset() 
  62. {
  63.     m_nLastState=-1;
  64.     m_nState=kForward;
  65.     m_Car.Reset();
  66.     m_bFinished=false;
  67.  
  68.     // Not sure if these are good default values, but for replays sake...
  69.     m_nWayPtIdx=1;
  70.     m_nCurMapCompType=3;
  71.     m_nCurMapCompIdx=1;
  72.     m_nDestMapCompType=3;
  73.     m_nDestMapCompId=1;
  74.     m_fTurnCenterPtOffset=0.f;
  75.     m_nNumWayPts=1;
  76.     m_nBestRoute=-1;
  77.     m_nNumRoutes=0;
  78.     m_bDir[0]=m_bDir[1]=m_bDir[2]=0;
  79.     m_fRdAngle[0]=m_fRdAngle[1]=0.f;
  80.     m_fTurn[0]=m_fTurn[1]=0.f;
  81.     m_LPos.Set(0.f,0.f,0.f);
  82.     m_LPosition.Set(0.f,0.f,0.f);
  83.     m_Destination.Set(0.f,0.f,0.f);
  84.     m_DestHeading.Set(0.f,0.f,0.f);
  85.     m_IntersectionPt[0].Set(0.f,0.f,0.f);
  86.     m_IntersectionPt[1].Set(0.f,0.f,0.f);
  87.     m_TurnCenterPt[0].Set(0.f,0.f,0.f);
  88.     m_TurnCenterPt[1].Set(0.f,0.f,0.f);
  89.     m_TurnStartDir[0].Set(0.f,0.f,0.f);
  90.     m_TurnStartDir[1].Set(0.f,0.f,0.f);
  91.     m_TurnEndDir[0].Set(0.f,0.f,0.f);
  92.     m_TurnEndDir[1].Set(0.f,0.f,0.f);
  93.     m_fTurnSetback[0]=m_fTurnSetback[1]=0.f;
  94.     m_fRadius[0]=m_fRadius[1]=0.f;
  95.  
  96.     m_fBrake=0.0f;
  97.     m_fThrottle=0.0f;
  98.     m_fSteering=0.0f;
  99.  
  100.     for(int i=0;i<3;i++)
  101.         m_pRoads[i]=0;
  102.  
  103.     for(int k=0;k<MAX_ROUTE_NODES;k++) {
  104.         m_Route[k].Reset();
  105.     }
  106.  
  107.     for(int j=0; j< MAX_NUM_ROUTES; j++) {
  108.         m_naNumRouteVerts[j]=0;
  109.         for(int i=0;i<MAX_ROUTE_NODES;i++) {
  110.             m_AltRoute[j][i].Reset();
  111.         }
  112.     }
  113. }
  114.  
  115. void aiNavigation::RegisterRoute(short *panRoute,short nNumRouteNodes,const Vector3& Destination,
  116.                                      const Vector3& DestHeading,int nNumRepeats,float fTargetSpeed,
  117.                                      float fDestOffset,float fTurnVelFactor,float fBrakeThreshold,
  118.                                      float fRoutePlanDist)
  119. {
  120.     aiIntersection *pInt0, *pInt1;
  121.  
  122.     m_fTurnVelFactor=fTurnVelFactor;
  123.     m_fBrakeThreshold=fBrakeThreshold;
  124.     m_fRoutePlanDist=fRoutePlanDist;
  125.     
  126.     m_panWayPts=panRoute;
  127.     m_nNumWayPts=nNumRouteNodes;
  128.     m_pPrevRoad=0;
  129.     m_LPosition=m_Car.GetMatrix().Pos;
  130.     m_LPos=m_Car.GetMatrix().Pos;
  131.     m_nBestRoute=-1;
  132.     m_TargetPt=m_Car.GetMatrix().Pos;
  133.  
  134.     m_pRoads[0]=0;
  135.     m_pRoads[1]=0;
  136.     m_pRoads[2]=0;
  137.  
  138.     m_fRdAngle[0]=0.f;
  139.     m_fRdAngle[1]=0.f;
  140.  
  141.     m_Route[0].m_nSVIdx=0;
  142.     m_Route[0].m_nSRd=0;
  143.  
  144.     m_Destination=Destination;
  145.     m_DestHeading=DestHeading;
  146.     m_fDestSpeed=fTargetSpeed;
  147.     m_fDestOffset=fDestOffset;
  148.     DestMapComponent(m_Destination,&m_nDestMapCompId,&m_nDestMapCompType);
  149.  
  150.     int nTargetId=-1;
  151.     if(m_pRoads[0])
  152.         nTargetId=m_pRoads[0]->Id();
  153.  
  154.     AIMAP.MapComponent(m_Car.GetMatrix().Pos,&m_nCurMapCompIdx,&m_nCurMapCompType,
  155.                        m_Car.GetRoomId(),nTargetId);
  156.     switch(m_nCurMapCompType)
  157.     {
  158.         case kIntersection:
  159.         {
  160.             m_nRdIdx=0;
  161.  
  162.             m_nWayPtIdx=0;
  163.             m_pRoads[0]=AIMAP.Intersection(m_nCurMapCompIdx)->Road(0);
  164.             if(m_pRoads[0]->Destination()==AIMAP.Intersection(m_nCurMapCompIdx))
  165.                 m_bDir[0]=1;    // The direction that the vehicle will traverse the road
  166.             else
  167.                 m_bDir[0]=0;
  168.  
  169.             if(m_nNumWayPts==2)
  170.             {
  171.                 pInt0=AIMAP.Intersection(CheckPt(0));
  172.                 pInt1=AIMAP.Intersection(CheckPt(1));
  173.                 m_pRoads[0]=AIMAP.DetRdSegBetweenInts(pInt0,pInt1,&m_bDir[0]);
  174.                 m_nWayPtIdx=1;
  175.                 m_nRdIdx=1;
  176.             }
  177.             else if(m_nNumWayPts==3)
  178.             {
  179.                 pInt0=AIMAP.Intersection(CheckPt(0));
  180.                 pInt1=AIMAP.Intersection(CheckPt(1));
  181.                 m_pRoads[0]=AIMAP.DetRdSegBetweenInts(pInt0,pInt1,&m_bDir[0]);
  182.                 pInt0=AIMAP.Intersection(CheckPt(1));
  183.                 pInt1=AIMAP.Intersection(CheckPt(2));
  184.                 m_pRoads[1]=AIMAP.DetRdSegBetweenInts(pInt0,pInt1,&m_bDir[1]);
  185.                 m_nWayPtIdx=1;
  186.                 m_nRdIdx=2;
  187.             }
  188.             else if(m_nNumWayPts>3)
  189.             {
  190.                 pInt0=AIMAP.Intersection(CheckPt(0));
  191.                 pInt1=AIMAP.Intersection(CheckPt(1));
  192.                 m_pRoads[0]=AIMAP.DetRdSegBetweenInts(pInt0,pInt1,&m_bDir[0]);
  193.                 pInt0=AIMAP.Intersection(CheckPt(1));
  194.                 pInt1=AIMAP.Intersection(CheckPt(2));
  195.                 m_pRoads[1]=AIMAP.DetRdSegBetweenInts(pInt0,pInt1,&m_bDir[1]);
  196.                 pInt0=AIMAP.Intersection(CheckPt(2));
  197.                 pInt1=AIMAP.Intersection(CheckPt(3));
  198.                 m_pRoads[2]=AIMAP.DetRdSegBetweenInts(pInt0,pInt1,&m_bDir[2]);
  199.                 m_nWayPtIdx=1;
  200.                 m_nRdIdx=3;
  201.             }
  202.  
  203.             m_nState=kForward;
  204.             if(m_bDir[0])
  205.             {
  206.                 Vector3 p=m_pRoads[0]->CenterVertice(0)-m_Car.GetMatrix().Pos;
  207.                 m_fSideDist=aiDot(p,m_pRoads[0]->VertXDir(0));
  208.             }
  209.             else
  210.             {
  211.                 int nNumVerts=m_pRoads[0]->NumVerts();
  212.                 Vector3 p=m_pRoads[0]->CenterVertice(nNumVerts-1)-m_Car.GetMatrix().Pos;
  213.                 m_fSideDist=aiDot(p,-m_pRoads[0]->VertXDir(nNumVerts-1));
  214.             }
  215.             break;
  216.         }
  217.         case kRoad:
  218.         {
  219.             int nCompIdx=m_nCurMapCompIdx;
  220.             m_nWayPtIdx=0;
  221.             m_bDir[0]=1;
  222.             m_pRoads[0]=AIMAP.Path(nCompIdx);
  223.  
  224.             m_nRdIdx=0;
  225.             if(m_nNumWayPts==2)
  226.             {
  227.                 pInt0=AIMAP.Intersection(CheckPt(0));
  228.                 pInt1=AIMAP.Intersection(CheckPt(1));
  229.                 m_pRoads[0]=AIMAP.DetRdSegBetweenInts(pInt0,pInt1,&m_bDir[0]);
  230.                 m_nWayPtIdx=1;
  231.                 m_nRdIdx=1;
  232.             }
  233.             else if(m_nNumWayPts==3)
  234.             {
  235.                 pInt0=AIMAP.Intersection(CheckPt(0));
  236.                 pInt1=AIMAP.Intersection(CheckPt(1));
  237.                 m_pRoads[0]=AIMAP.DetRdSegBetweenInts(pInt0,pInt1,&m_bDir[0]);
  238.                 pInt0=AIMAP.Intersection(CheckPt(1));
  239.                 pInt1=AIMAP.Intersection(CheckPt(2));
  240.                 m_pRoads[1]=AIMAP.DetRdSegBetweenInts(pInt0,pInt1,&m_bDir[1]);
  241.                 m_nWayPtIdx=1;
  242.                 m_nRdIdx=2;
  243.             }
  244.             else if(m_nNumWayPts>3)
  245.             {
  246.                 pInt0=AIMAP.Intersection(CheckPt(0));
  247.                 pInt1=AIMAP.Intersection(CheckPt(1));
  248.                 m_pRoads[0]=AIMAP.DetRdSegBetweenInts(pInt0,pInt1,&m_bDir[0]);
  249.                 pInt0=AIMAP.Intersection(CheckPt(1));
  250.                 pInt1=AIMAP.Intersection(CheckPt(2));
  251.                 m_pRoads[1]=AIMAP.DetRdSegBetweenInts(pInt0,pInt1,&m_bDir[1]);
  252.                 pInt0=AIMAP.Intersection(CheckPt(2));
  253.                 pInt1=AIMAP.Intersection(CheckPt(3));
  254.                 m_pRoads[2]=AIMAP.DetRdSegBetweenInts(pInt0,pInt1,&m_bDir[2]);
  255.                 m_nWayPtIdx=1;
  256.                 m_nRdIdx=3;
  257.             }
  258.  
  259.             m_nState=kForward;
  260.             int nVIdx=aiClamp(m_pRoads[0]->RoadVertice(m_Car.GetMatrix().Pos,m_bDir[0],-1),
  261.                             0,m_pRoads[0]->NumVerts()-1);
  262.             if(m_bDir[0])
  263.             {
  264.                 Vector3 p=m_pRoads[0]->CenterVertice(nVIdx)-m_Car.GetMatrix().Pos;
  265.                 m_fSideDist=aiDot(p,m_pRoads[0]->VertXDir(nVIdx));
  266.             }
  267.             else
  268.             {
  269.                 int nNumVerts=m_pRoads[0]->NumVerts();
  270.                 Vector3 p=m_pRoads[0]->CenterVertice(nNumVerts-nVIdx-1)-m_Car.GetMatrix().Pos;
  271.                 m_fSideDist=aiDot(p,-m_pRoads[0]->VertXDir(nNumVerts-nVIdx-1));
  272.             }
  273.             break;
  274.         }
  275.     }
  276. }
  277.  
  278. void aiNavigation::DriveRoute()
  279. {
  280.     switch(m_nState)
  281.     {
  282.         case kBackup:
  283.         {
  284.             if(m_nLastState!=m_nState)
  285.             {
  286.                 InitBackup();
  287.                 m_nLastState=m_nState;
  288.             }
  289.             Backup();
  290.             break;
  291.         }
  292.         case kForward:
  293.         {
  294.             if(m_nLastState!=m_nState)
  295.             {
  296.                 InitForward();
  297.                 m_nLastState=m_nState;
  298.             }
  299.             Forward();
  300.             break;
  301.         }
  302.         case kStop:
  303.         {
  304.             Stop();
  305.             break;
  306.         }
  307.     }
  308. }
  309.  
  310. void aiNavigation::Stop()
  311. {
  312.     m_TargetPt=m_Destination;
  313.     Vector3 TargetHeading=m_Destination-m_Car.GetMatrix().Pos;
  314.     float fX=aiDot(TargetHeading,m_Car.GetMatrix().XDir);
  315.     float fZ=aiDot(TargetHeading,m_Car.GetMatrix().ZDir);
  316.     float fAngle=atan2f(fX,-fZ)*1.33f;
  317.     m_fSteering=aiClamp(fAngle,-1.f,1.f);
  318.     m_Car.SetSteering(m_fSteering);
  319.  
  320.     m_fThrottle=0.f;
  321.     m_fBrake=1.f;
  322.     m_Car.SetThrottle(0.f);
  323.     m_Car.SetBrake(1.f);
  324.  
  325.     m_naNumRouteVerts[0]=0;
  326.     m_nNumRoutes=0;
  327. }
  328.  
  329. void aiNavigation::InitForward()
  330. {
  331.     m_fSteering=0.f;
  332.     m_Car.SetSteering(0.f);
  333.     m_Car.SetThrottle(0.f);
  334.     m_Car.SetBrake(0.f);
  335.     m_TargetPt=m_Car.GetMatrix().Pos;
  336.  
  337.     m_nNumRoutes=0;
  338.     m_nBestRoute=-1;
  339.     m_fTurnCenterPtOffset=0.f;
  340.     for(int ctr=0;ctr<MAX_NUM_ROUTES;ctr++)
  341.     {
  342.         m_naOffRoadRoute[ctr]=0;
  343.         m_naBlockedRoute[ctr]=0;
  344.         m_naNumRouteVerts[ctr]=0;
  345.     }
  346. }
  347.  
  348. void aiNavigation::Forward()
  349. {
  350.     if(m_Car.Stuck())
  351.     {
  352.         PlanRoute();
  353.         m_nState=kBackup;
  354.         return;
  355.     }
  356.  
  357.     // Determine our road segment and lane
  358.     short nTargetRdId=-1;
  359.     if(m_nWayPtIdx<m_nNumWayPts)
  360.     {
  361.         bool bDir;
  362.         if(m_nWayPtIdx==0)
  363.         {
  364.             aiIntersection *pInt0=AIMAP.Intersection(CheckPt(m_nNumWayPts-1));
  365.             aiIntersection *pInt1=AIMAP.Intersection(CheckPt(m_nWayPtIdx));
  366.             aiPath *pCurRoad=AIMAP.DetRdSegBetweenInts(pInt0,pInt1,&bDir);
  367.             if(pCurRoad)
  368.                 nTargetRdId=pCurRoad->Id();
  369.         }
  370.         else
  371.         {
  372.             aiIntersection *pInt0=AIMAP.Intersection(CheckPt(m_nWayPtIdx-1));
  373.             aiIntersection *pInt1=AIMAP.Intersection(CheckPt(m_nWayPtIdx));
  374.             aiPath *pCurRoad=AIMAP.DetRdSegBetweenInts(pInt0,pInt1,&bDir);
  375.             if(pCurRoad)
  376.                 nTargetRdId=pCurRoad->Id();
  377.         }
  378.     }
  379.     else
  380.     {
  381.         if(m_nDestMapCompType==kRoad)
  382.         {
  383.             nTargetRdId=m_nDestMapCompId;
  384.         }
  385.     }
  386.     AIMAP.MapComponent(m_Car.GetMatrix().Pos,&m_nCurMapCompIdx,
  387.                        &m_nCurMapCompType,m_Car.GetRoomId(),nTargetRdId);
  388.     SolveRoadTargetPoint();
  389.  
  390.     // if the destination is behind me, and the dest heading is similar
  391.     Vector3 TargetHeading;
  392.     if(m_AltRoute[m_nBestRoute][1].m_fCumDist==9999.f &&        // going for the destination
  393.        aiDist2(m_Car.GetMatrix().Pos,m_Destination)<625.f &&
  394.        aiDot(-m_Car.GetMatrix().ZDir,m_DestHeading)>0.f &&
  395.        aiDot((m_Destination-m_Car.GetMatrix().Pos),-m_Car.GetMatrix().ZDir)<0.f)
  396.     {
  397.         TargetHeading=-(m_TargetPt-m_Car.GetMatrix().Pos);
  398.         m_fBrake=1.f;
  399.         m_fThrottle=0.f;
  400.     }
  401.     else
  402.     {
  403.         TargetHeading=m_TargetPt-m_Car.GetMatrix().Pos;
  404.     }
  405.  
  406.     float fX=aiDot(TargetHeading,m_Car.GetMatrix().XDir);
  407.     float fZ=aiDot(TargetHeading,m_Car.GetMatrix().ZDir);
  408.     float fAngle=atan2f(fX,-fZ)*1.33f;
  409.  
  410.     fAngle*=1.428f;
  411.     m_fSteering=aiClamp(fAngle,-1.f,1.f);
  412.  
  413.     m_Car.SetSteering(m_fSteering);
  414.     m_Car.SetBrake(m_fBrake);
  415.     m_Car.SetThrottle(m_fThrottle);
  416. }
  417.  
  418. void aiNavigation::InitBackup()
  419. {
  420.     m_nBackupCount=0;
  421.     m_TargetPt=m_AltRoute[m_nBestRoute][1].Vtx;
  422.     m_Car.SetTransmissionToReverse();
  423. }
  424.  
  425. void aiNavigation::Backup()
  426. {
  427.     // Calc turn angle to reach the target.
  428.     Vector3 TargetHeading=m_TargetPt-m_Car.GetMatrix().Pos;
  429.  
  430.     float fX=TargetHeading^m_Car.GetMatrix().XDir;
  431.     float fZ=TargetHeading^-m_Car.GetMatrix().ZDir;
  432.  
  433.     float fAngle=atan2f(fX,fZ);
  434.     if(fAngle>0.1f || fAngle<-.1f)
  435.     {
  436.         float fSteering=-fAngle/.35f;
  437.         fSteering=aiClamp(fSteering,-1.f,1.f);
  438.         m_Car.SetSteering(fSteering);
  439.     }
  440.     else
  441.     {
  442.         m_Car.SetSteering(0.f);
  443.         FinishedBackingUp();
  444.         return;
  445.     }
  446.  
  447.     if(m_nBackupCount>65)
  448.     {
  449.         FinishedBackingUp();
  450.         return;
  451.     }
  452.     else
  453.     {
  454.         m_Car.SetThrottle(0.85f);
  455.         m_Car.SetBrake(0.f);
  456.     }
  457.     m_nBackupCount++;
  458. }
  459.  
  460. void aiNavigation::FinishedBackingUp()
  461. {
  462.     // Restore the state before the backup.
  463.     m_nState=kForward;
  464.     m_Car.SetThrottle(0.0f);
  465.     m_Car.SetBrake(1.f);
  466. }
  467.  
  468. //------Privates-----------------------------------------------------------------
  469.  
  470. void aiNavigation::CalcSpeed()
  471. {
  472.     m_fThrottle=1.f;
  473.     m_fBrake=0.f;
  474.  
  475.     // Check if near the final destination which occurs at the end of the race.
  476.     float fDist=aiDist2(m_Car.GetMatrix().Pos,m_Destination);
  477.     if(fDist<5000.f && m_nWayPtIdx>=m_nNumWayPts)
  478.     {
  479.         float fTargetVel=m_fDestSpeed;
  480.         float fDistToTarget=(float)sqrt(fDist)-m_fDestOffset;
  481.         float fTargetTime=fDistToTarget/m_Car.Speed();
  482.         float fBrake=0.f;
  483.         if(m_Car.Speed()>fTargetVel)
  484.             fBrake=(m_Car.Speed()-fTargetVel)/(fTargetTime*_fFriction*19.8f);
  485.  
  486.         if(fBrake>fDistToTarget*.014)
  487.         {
  488.             m_fThrottle=0.f;
  489.             m_fBrake=fBrake;
  490.             m_fBrake=aiClamp(m_fBrake,0.f,1.f);
  491.         }
  492.         if(fDistToTarget<2.5f && m_fDestSpeed<=0.f)
  493.         {
  494.             m_fThrottle=0.f;
  495.             m_fBrake=1.f;
  496.         }
  497.         return;
  498.     }
  499.  
  500.     // Check the intersection turns.
  501.     if(m_pRoads[0] && (m_fRdAngle[0]<-_fTurnThreshold || _fTurnThreshold<m_fRdAngle[0]) &&
  502.        aiDist2(m_Car.GetMatrix().Pos,m_IntersectionPt[0])<CheckDistance(0))
  503.     {
  504.         float fTargetVel=(float)sqrt(_fFriction*19.8f*m_fRadius[0])*
  505.                          m_fTurnVelFactor;
  506.  
  507.         // Set the brake and throttle
  508.         float fBrake=0.f;
  509.         if(m_Car.Speed()>fTargetVel)
  510.         {
  511.             Vector3 p=m_AltRoute[m_nBestRoute][0].Vtx-m_IntersectionPt[0];
  512.             float fDistToTurn;
  513.             if(m_bDir[0])
  514.                 fDistToTurn=aiDot(p,m_pRoads[0]->VertZDir(m_pRoads[0]->NumVerts()-1))-m_fTurnSetback[0];
  515.             else
  516.                 fDistToTurn=aiDot(p,-m_pRoads[0]->VertZDir(0))-m_fTurnSetback[0];
  517.  
  518.             float fTargetTime=fDistToTurn/m_Car.Speed();
  519.  
  520.             fBrake=(m_Car.Speed()-fTargetVel)/(fTargetTime*_fFriction*19.8f);
  521.         }
  522.  
  523.         if(fBrake>m_fBrakeThreshold && fBrake>m_fBrake)
  524.         {
  525.             m_fThrottle=0.f;
  526.             m_fBrake=fBrake;
  527.             m_fBrake=aiClamp(m_fBrake,0.f,1.f);
  528.         }
  529.     }
  530.     else if(m_pRoads[1] && (m_fRdAngle[1]<-_fTurnThreshold || _fTurnThreshold<m_fRdAngle[1]) &&
  531.             aiDist2(m_Car.GetMatrix().Pos,m_IntersectionPt[1])<CheckDistance(1))
  532.     {
  533.         float fTargetVel=(float)sqrt(_fFriction*19.8f*m_fRadius[1])*
  534.                          m_fTurnVelFactor;
  535.  
  536.         // Set the brake and throttle
  537.         float fBrake=0.f;
  538.         if(m_Car.Speed()>fTargetVel)
  539.         {
  540.             float fDistToTurn=aiDist(m_AltRoute[m_nBestRoute][0].Vtx,m_IntersectionPt[1])-m_fTurnSetback[1];
  541.                 
  542.             float fTargetTime=fDistToTurn/m_Car.Speed();
  543.             fBrake=(m_Car.Speed()-fTargetVel)/(fTargetTime*_fFriction*19.8f);
  544.         }
  545.  
  546.         if(fBrake>m_fBrakeThreshold && fBrake>m_fBrake)
  547.         {
  548.             m_fThrottle=0.f;
  549.             m_fBrake=fBrake;
  550.             m_fBrake=aiClamp(m_fBrake,0.f,1.f);
  551.         }
  552.     }
  553. }
  554.  
  555. float aiNavigation::CheckDistance(int nId)
  556. {
  557.     return (m_fRoutePlanDist+m_fTurnSetback[nId])*(m_fRoutePlanDist+m_fTurnSetback[nId]);
  558. }
  559.  
  560. void aiNavigation::SolveRoadTargetPoint()
  561. {
  562.     if(m_nWayPtIdx>=m_nNumWayPts)
  563.         m_bFinished=true;
  564.     else
  565.         m_bFinished=false;
  566.  
  567.     // Plan out a rough route consisting of the next three road segs
  568.     if(PlanRoute())
  569.         return;
  570.  
  571.     // Process all the things that need to done when changing roads.
  572.     if(m_pPrevRoad!=m_pRoads[0])
  573.     {
  574.         m_LPos=m_Car.GetMatrix().Pos;
  575.         m_LPosition=m_Car.GetMatrix().Pos;
  576.         m_pPrevRoad=m_pRoads[0];
  577.         InitRoadTurns();
  578.         CalcRoadTurns();
  579.     }
  580.  
  581.     // Calc the exact route through the traffic
  582.     CalcRoute();
  583.  
  584.     m_TargetPt=m_AltRoute[m_nBestRoute][1].Vtx;
  585.  
  586.     // Set the throttle and brake
  587.     CalcSpeed();
  588. }
  589.  
  590. int aiNavigation::PlanRoute()
  591. {
  592.     // Check if time to increment the destination intersection index
  593.     if(m_nWayPtIdx<m_nNumWayPts && 
  594.       (m_nCurMapCompType==kIntersection && m_nCurMapCompIdx==CheckPt(m_nWayPtIdx)))
  595.     {
  596.         // Do this in case the road leading to this intersection is very short.
  597.         // i.e. two intersections which are practically next to each other. What
  598.         // happens is the opponents will go from one int to another without ever
  599.         // registering on the conecting road.
  600.         if(m_nWayPtIdx>0)
  601.         {
  602.             bool bDir;
  603.             aiPath *pPath=AIMAP.DetRdSegBetweenInts(AIMAP.Intersection(CheckPt(m_nWayPtIdx-1)),
  604.                                                     AIMAP.Intersection(CheckPt(m_nWayPtIdx)),&bDir);
  605.             if(m_pRoads[0]!=pPath)
  606.                 LocateWayPtFromRoad(pPath);
  607.         }
  608.  
  609.         // Lets move on to the next intersection.
  610.         m_nWayPtIdx++;
  611.     }
  612.  
  613.     switch(m_nCurMapCompType)
  614.     {
  615.         case kRoad:
  616.         {
  617.             LocateWayPtFromRoad(AIMAP.Path(m_nCurMapCompIdx));
  618.             break;
  619.         }
  620.         case kIntersection:
  621.         {
  622.             if(m_nRdIdx<3)
  623.             {
  624.                 if(m_nDestMapCompType==kRoad)
  625.                 {
  626.                     if(m_nNumWayPts>0)
  627.                     {
  628.                         for(int i=0;i<AIMAP.Intersection(m_panWayPts[m_nNumWayPts-1])->NumRoads();i++)
  629.                         {
  630.                             if(AIMAP.Intersection(m_panWayPts[m_nNumWayPts-1])->Road(i)==
  631.                                AIMAP.Path(m_nDestMapCompId))
  632.                             {
  633.                                 m_pRoads[m_nRdIdx]=AIMAP.Intersection(m_panWayPts[m_nNumWayPts-1])->Road(i);
  634.                                 if(m_pRoads[m_nRdIdx]->Departure()==AIMAP.Intersection(m_panWayPts[m_nNumWayPts-1]))
  635.                                     m_bDir[m_nRdIdx]=1;
  636.                                 else
  637.                                     m_bDir[m_nRdIdx]=0;
  638.                                 return 0;
  639.                             }
  640.                         }
  641.                     }
  642.                     else
  643.                     {
  644.                         for(int i=0;i<AIMAP.Intersection(m_nCurMapCompIdx)->NumRoads();i++)
  645.                         {
  646.                             if(AIMAP.Intersection(m_nCurMapCompIdx)->Road(i)==
  647.                                AIMAP.Path(m_nDestMapCompId))
  648.                             {
  649.                                 m_pRoads[m_nRdIdx]=AIMAP.Intersection(m_nCurMapCompIdx)->Road(i);
  650.                                 if(m_pRoads[m_nRdIdx]->Departure()==AIMAP.Intersection(m_nCurMapCompIdx))
  651.                                     m_bDir[m_nRdIdx]=1;
  652.                                 else
  653.                                     m_bDir[m_nRdIdx]=0;
  654.                                 return 0;
  655.                             }
  656.                         }
  657.                     }
  658.                 }
  659.             }
  660.         }
  661.     }
  662.     return 0;
  663. }
  664.  
  665. int aiNavigation::LocateWayPtFromRoad(aiPath *pCallLink)
  666. {
  667.     if(m_nWayPtIdx>=m_nNumWayPts)
  668.     {
  669.         m_pRoads[0]=pCallLink;
  670.         m_pRoads[1]=m_pRoads[2]=0;
  671.  
  672.         int nIdx=m_pRoads[0]->RoadVertice(m_Car.GetMatrix().Pos,1,-1);
  673.         nIdx=aiClamp(nIdx,0,m_pRoads[0]->NumVerts()-1);
  674.         float fDir=aiDot(m_pRoads[0]->VertZDir(nIdx),m_Car.GetMatrix().ZDir);
  675.         if(fDir>=0.f)
  676.             m_bDir[0]=1;
  677.         else
  678.             m_bDir[0]=0;
  679.         return true;
  680.     }
  681.  
  682.     // Is the destination int the next goal? Handle missed intersections
  683.     if(pCallLink->Destination()->Id()==CheckPt(m_nWayPtIdx+1) ||
  684.        pCallLink->Departure()->Id()==CheckPt(m_nWayPtIdx+1) ||
  685.        (m_nWayPtIdx==m_nNumWayPts-1 && m_nDestMapCompId==m_nCurMapCompIdx))
  686.         m_nWayPtIdx++;
  687.  
  688.     // Is the destination int the current goal?
  689.     if(pCallLink->Destination()->Id()==CheckPt(m_nWayPtIdx))
  690.     {
  691.         m_bDir[0]=1;
  692.         m_pRoads[0]=pCallLink;
  693.  
  694.         m_nRdIdx=0;
  695.         aiIntersection *pInt0, *pInt1, *pInt2;
  696.         if(m_nWayPtIdx==m_nNumWayPts-1) 
  697.         {
  698.             pInt0=AIMAP.Intersection(CheckPt(m_nWayPtIdx));
  699.             pInt1=0;
  700.             pInt2=0;
  701.             m_nRdIdx=1;
  702.         }
  703.         else if(m_nWayPtIdx==m_nNumWayPts-2)
  704.         {
  705.             pInt0=AIMAP.Intersection(CheckPt(m_nWayPtIdx));
  706.             pInt1=AIMAP.Intersection(CheckPt(m_nWayPtIdx+1));
  707.             pInt2=0;
  708.             m_nRdIdx=2;
  709.         }
  710.         else
  711.         {
  712.             pInt0=AIMAP.Intersection(CheckPt(m_nWayPtIdx));
  713.             pInt1=AIMAP.Intersection(CheckPt(m_nWayPtIdx+1));
  714.             pInt2=AIMAP.Intersection(CheckPt(m_nWayPtIdx+2));
  715.             m_nRdIdx=3;
  716.         }
  717.  
  718.         m_pRoads[1]=AIMAP.DetRdSegBetweenInts(pInt0,pInt1,&m_bDir[1]);
  719.         m_pRoads[2]=AIMAP.DetRdSegBetweenInts(pInt1,pInt2,&m_bDir[2]);
  720.  
  721.         if(m_nDestMapCompType==kRoad && 0<m_nRdIdx && m_nRdIdx<3)
  722.         {
  723.             for(int i=0;i<AIMAP.Intersection(m_panWayPts[m_nNumWayPts-1])->NumRoads();i++)
  724.             {
  725.                 if(AIMAP.Intersection(m_panWayPts[m_nNumWayPts-1])->Road(i)==
  726.                    AIMAP.Path(m_nDestMapCompId))
  727.                 {
  728.                     m_pRoads[m_nRdIdx]=AIMAP.Intersection(m_panWayPts[m_nNumWayPts-1])->Road(i);
  729.                     if(m_pRoads[m_nRdIdx]->Departure()==
  730.                        AIMAP.Intersection(m_panWayPts[m_nNumWayPts-1]))
  731.                         m_bDir[m_nRdIdx]=1;
  732.                     else
  733.                         m_bDir[m_nRdIdx]=0;
  734.                     break;
  735.                 }
  736.             }
  737.         }
  738.         return true;
  739.     }
  740.  
  741.     // Check for wrong way down a one way road.
  742.     if(pCallLink->Departure()->Id()==CheckPt(m_nWayPtIdx)) 
  743.     {
  744.         m_bDir[0]=0;
  745.         m_pRoads[0]=pCallLink;
  746.  
  747.         m_nRdIdx=0;
  748.         aiIntersection *pInt0, *pInt1, *pInt2;
  749.         if(m_nWayPtIdx==m_nNumWayPts-1) 
  750.         {
  751.             pInt0=AIMAP.Intersection(CheckPt(m_nWayPtIdx));
  752.             pInt1=0;
  753.             pInt2=0;
  754.             m_nRdIdx=1;
  755.         }
  756.         else if(m_nWayPtIdx==m_nNumWayPts-2)
  757.         {
  758.             pInt0=AIMAP.Intersection(CheckPt(m_nWayPtIdx));
  759.             pInt1=AIMAP.Intersection(CheckPt(m_nWayPtIdx+1));
  760.             pInt2=0;
  761.             m_nRdIdx=2;
  762.         }
  763.         else
  764.         {
  765.             pInt0=AIMAP.Intersection(CheckPt(m_nWayPtIdx));
  766.             pInt1=AIMAP.Intersection(CheckPt(m_nWayPtIdx+1));
  767.             pInt2=AIMAP.Intersection(CheckPt(m_nWayPtIdx+2));
  768.             m_nRdIdx=3;
  769.         }
  770.  
  771.         m_pRoads[1]=AIMAP.DetRdSegBetweenInts(pInt0,pInt1,&m_bDir[1]);
  772.         m_pRoads[2]=AIMAP.DetRdSegBetweenInts(pInt1,pInt2,&m_bDir[2]);
  773.  
  774.         if(m_nDestMapCompType==kRoad && 0<m_nRdIdx && m_nRdIdx<3)
  775.         {
  776.             for(int i=0;i<AIMAP.Intersection(m_panWayPts[m_nNumWayPts-1])->NumRoads();i++)
  777.             {
  778.                 if(AIMAP.Intersection(m_panWayPts[m_nNumWayPts-1])->Road(i)==
  779.                    AIMAP.Path(m_nDestMapCompId))
  780.                 {
  781.                     m_pRoads[m_nRdIdx]=AIMAP.Intersection(m_panWayPts[m_nNumWayPts-1])->Road(i);
  782.                     if(m_pRoads[m_nRdIdx]->Departure()==
  783.                        AIMAP.Intersection(m_panWayPts[m_nNumWayPts-1]))
  784.                         m_bDir[m_nRdIdx]=1;
  785.                     else
  786.                         m_bDir[m_nRdIdx]=0;
  787.                     break;
  788.                 }
  789.             }
  790.         }
  791.         return true;
  792.     }
  793.  
  794.     // Check the destination intersection's intersections
  795.     for(int ctr=0;ctr<pCallLink->Destination()->NumRoads();ctr++)
  796.     {
  797.         if(pCallLink->Destination()->Road(ctr)->Destination()->Id()==CheckPt(m_nWayPtIdx))
  798.         {
  799.             m_pRoads[0]=pCallLink;
  800.             m_bDir[0]=1;
  801.             m_pRoads[1]=pCallLink->Destination()->Road(ctr);
  802.             m_bDir[1]=1;
  803.             if(m_nWayPtIdx+1<m_nNumWayPts)
  804.             {
  805.                 aiIntersection *pInt0=AIMAP.Intersection(CheckPt(m_nWayPtIdx));
  806.                 aiIntersection *pInt1=AIMAP.Intersection(CheckPt(m_nWayPtIdx+1));
  807.                 m_pRoads[2]=AIMAP.DetRdSegBetweenInts(pInt0,pInt1,&m_bDir[2]);
  808.             }
  809.             else
  810.                 m_pRoads[2]=0;
  811.             return true;
  812.         }
  813.         if(pCallLink->Destination()->Road(ctr)->Departure()->Id()==CheckPt(m_nWayPtIdx))
  814.         {
  815.             m_pRoads[0]=pCallLink;
  816.             m_bDir[0]=1;
  817.             m_pRoads[1]=pCallLink->Destination()->Road(ctr);
  818.             m_bDir[1]=0;
  819.             if(m_nWayPtIdx+1<m_nNumWayPts)
  820.             {
  821.                 aiIntersection *pInt0=AIMAP.Intersection(CheckPt(m_nWayPtIdx));
  822.                 aiIntersection *pInt1=AIMAP.Intersection(CheckPt(m_nWayPtIdx+1));
  823.                 m_pRoads[2]=AIMAP.DetRdSegBetweenInts(pInt0,pInt1,&m_bDir[2]);
  824.             }
  825.             else
  826.                 m_pRoads[2]=0;
  827.             return true;
  828.         }
  829.     }
  830.     
  831.     // Check the departure intersection's intersections
  832.     for(int ctr=0;ctr<pCallLink->Departure()->NumRoads();ctr++)
  833.     {
  834.         if(pCallLink->Departure()->Road(ctr)->Destination()->Id()==CheckPt(m_nWayPtIdx))
  835.         {
  836.             m_pRoads[0]=pCallLink;
  837.             m_bDir[0]=0;
  838.             m_pRoads[1]=pCallLink->Departure()->Road(ctr);
  839.             m_bDir[1]=1;
  840.             if(m_nWayPtIdx+1<m_nNumWayPts)
  841.             {
  842.                 aiIntersection *pInt0=AIMAP.Intersection(CheckPt(m_nWayPtIdx));
  843.                 aiIntersection *pInt1=AIMAP.Intersection(CheckPt(m_nWayPtIdx+1));
  844.                 m_pRoads[2]=AIMAP.DetRdSegBetweenInts(pInt0,pInt1,&m_bDir[2]);
  845.             }
  846.             else
  847.                 m_pRoads[2]=0;
  848.             return true;
  849.         }
  850.         if(pCallLink->Departure()->Road(ctr)->Departure()->Id()==CheckPt(m_nWayPtIdx))
  851.         {
  852.             m_pRoads[0]=pCallLink;
  853.             m_bDir[0]=0;
  854.             m_pRoads[1]=pCallLink->Departure()->Road(ctr);
  855.             m_bDir[1]=0;
  856.             if(m_nWayPtIdx+1<m_nNumWayPts)
  857.             {
  858.                 aiIntersection *pInt0=AIMAP.Intersection(CheckPt(m_nWayPtIdx));
  859.                 aiIntersection *pInt1=AIMAP.Intersection(CheckPt(m_nWayPtIdx+1));
  860.                 m_pRoads[2]=AIMAP.DetRdSegBetweenInts(pInt0,pInt1,&m_bDir[2]);
  861.             }
  862.             else
  863.                 m_pRoads[2]=0;
  864.             return true;
  865.         }
  866.     }
  867.  
  868.     return false;
  869. }
  870.  
  871. void aiNavigation::CalcRoute()
  872. {
  873.     // Initialize the route
  874.     m_Route[0].Vtx=m_Car.GetMatrix().Pos;
  875.     m_Route[0].Vtx.y+=1.f;
  876.     m_Route[0].m_fCumAngle=0.f;
  877.     m_Route[0].m_fCumDist=0.f;
  878.     m_Route[0].m_nTurnId=0;
  879.  
  880.     if(!m_pRoads[0])
  881.     {
  882.         m_nBestRoute=0;
  883.         m_AltRoute[m_nBestRoute][1].Vtx=m_Destination;
  884.         m_AltRoute[m_nBestRoute][1].m_nTurnId=0;
  885.         return;
  886.     }
  887.  
  888.     m_Route[0].m_nSVIdx=m_pRoads[0]->RoadVertice(m_Car.GetMatrix().Pos,m_bDir[0],-1);
  889.     if(m_Route[0].m_nSVIdx==m_pRoads[0]->NumVerts())
  890.     {
  891.         // Check if we're coming up to a sharp turn.
  892.         if(m_pRoads[1])
  893.         {
  894.             m_Route[0].m_nSRd=1;
  895.             m_Route[0].m_nSVIdx=m_pRoads[1]->RoadVertice(m_Car.GetMatrix().Pos,m_bDir[1],-1);
  896.             if(m_Route[0].m_nSVIdx==m_pRoads[1]->NumVerts())
  897.             {
  898.                 if(m_pRoads[2])
  899.                 {
  900.                     m_Route[0].m_nSRd=2;
  901.                     m_Route[0].m_nSVIdx=m_pRoads[2]->RoadVertice(m_Car.GetMatrix().Pos,m_bDir[2],-1);
  902.                 }
  903.                 else
  904.                 {
  905.                     m_Route[0].m_nSVIdx=m_pRoads[1]->NumVerts()-1;
  906.                 }
  907.             }
  908.         }
  909.         else
  910.         {
  911.             m_Route[0].m_nSRd=0;
  912.             m_Route[0].m_nSVIdx=m_pRoads[0]->NumVerts()-1;
  913.         }
  914.     }
  915.     else
  916.     {
  917.         m_Route[0].m_nSRd=0;
  918.     }
  919.  
  920.     // reset vars
  921.     m_nNumRoutes=0;
  922.     m_fTurnCenterPtOffset=0.f;
  923.     for(int ctr=0;ctr<MAX_NUM_ROUTES;ctr++)
  924.     {
  925.         m_naOffRoadRoute[ctr]=0;
  926.         m_naBlockedRoute[ctr]=0;
  927.     }
  928.  
  929.     if(m_nWayPtIdx<m_nNumWayPts && !InSharpTurn(0))
  930.     {
  931.         m_Route[0].m_nType=kRoadTarget;
  932.         m_LPosition=m_Car.GetMatrix().Pos;
  933.     }
  934.     else
  935.     {
  936.         m_Route[0].m_nType=kTurnTarget;
  937.     }
  938.  
  939.     m_LPos=m_LPosition;
  940.     CalcRoadTurns();
  941.         
  942.     // Plot a route for the next x meters.
  943.     EnumRoutes(1);
  944.  
  945.     // Determine the best route. 
  946.     DetermineBestRoute();
  947. }
  948.  
  949. void aiNavigation::DetermineBestRoute()
  950. {
  951.     // - First, Pick the straightest route which stays on the road, and doesn't have any 
  952.     //   blocked targets.
  953.     // - Next, Pick the straightest route which stays on the road.
  954.     // - Next, pick the straightest route.
  955.     m_nBestRoute=-1;
  956.     float fBestCumAngle=99999.f;
  957.  
  958.     for(int ctr=0;ctr<m_nNumRoutes;ctr++)
  959.     {
  960.         if(!m_naOffRoadRoute[ctr] && !m_naBlockedRoute[ctr] &&
  961.            m_AltRoute[ctr][m_naNumRouteVerts[ctr]-1].m_fCumAngle<fBestCumAngle)
  962.         {
  963.             fBestCumAngle=m_AltRoute[ctr][m_naNumRouteVerts[ctr]-1].m_fCumAngle;
  964.             m_nBestRoute=ctr;
  965.         }
  966.     }
  967.  
  968.     if(m_nBestRoute<0)
  969.     {
  970.         fBestCumAngle=99999.f;
  971.         for(int ctr=0;ctr<m_nNumRoutes;ctr++)
  972.         {
  973.             if(!m_naOffRoadRoute[ctr] && 
  974.                 m_AltRoute[ctr][m_naNumRouteVerts[ctr]-1].m_fCumAngle<fBestCumAngle)
  975.             {
  976.                 fBestCumAngle=m_AltRoute[ctr][m_naNumRouteVerts[ctr]-1].m_fCumAngle;
  977.                 m_nBestRoute=ctr;
  978.             }
  979.         }
  980.     }
  981.  
  982.     if(m_nBestRoute<0)
  983.     {
  984.         fBestCumAngle=99999.f;
  985.         for(int ctr=0;ctr<m_nNumRoutes;ctr++)
  986.         {
  987.             if(m_AltRoute[ctr][m_naNumRouteVerts[ctr]-1].m_fCumAngle<fBestCumAngle)
  988.             {
  989.                 fBestCumAngle=m_AltRoute[ctr][m_naNumRouteVerts[ctr]-1].m_fCumAngle;
  990.                 m_nBestRoute=ctr;
  991.             }
  992.         }
  993.     }
  994. }
  995.  
  996. void aiNavigation::EnumRoutes(int nRouteIdx)
  997. {
  998.     Vector3 Position=m_Route[nRouteIdx-1].Vtx;
  999.     if(m_bFinished)        // Are we at the end of the route?
  1000.     {
  1001.         if(m_pRoads[m_Route[nRouteIdx-1].m_nSRd])
  1002.         {
  1003.             if(int nTurnId=InSharpTurn(nRouteIdx-1))
  1004.             {
  1005.                 m_Route[nRouteIdx-1].m_nTurnId=nTurnId-1;
  1006.                 int nWhichHalf=CalcSharpTurnTarget(nRouteIdx,nRouteIdx-1,Position);
  1007.                 if(nWhichHalf)
  1008.                 {
  1009.                     float fTmpSideDist;
  1010.                     m_Route[nRouteIdx].m_nTurnId++;
  1011.                     m_LPos=m_Route[nRouteIdx].Vtx;
  1012.                     if(m_pRoads[m_Route[nRouteIdx].m_nSRd] &&
  1013.                        m_pRoads[m_Route[nRouteIdx].m_nSRd]->IsPosOnRoad(m_Route[nRouteIdx].Vtx,2.f,&fTmpSideDist)>1)
  1014.                         CalcDestinationTarget(nRouteIdx,Position);
  1015.                 }
  1016.             }
  1017.             else
  1018.             {
  1019.                 m_LPos=m_Route[nRouteIdx-1].Vtx;
  1020.                 CalcDestinationTarget(nRouteIdx,Position);
  1021.                 if(int nTurnId=InSharpTurn(nRouteIdx))
  1022.                 {
  1023.                     m_Route[nRouteIdx].m_nTurnId=nTurnId-1;
  1024.                     CalcSharpTurnTarget(nRouteIdx,nRouteIdx,Position);
  1025.                 }
  1026.             }
  1027.         }
  1028.         else
  1029.         {
  1030.             SetTargetPtToDestination(nRouteIdx);
  1031.         }
  1032.     }
  1033.     else
  1034.     {
  1035.         if(int nTurnId=InSharpTurn(nRouteIdx-1))
  1036.         {
  1037.             m_Route[nRouteIdx-1].m_nTurnId=nTurnId-1;
  1038.             CalcSharpTurnTarget(nRouteIdx,nRouteIdx-1,Position);
  1039.         }
  1040.         else
  1041.         {
  1042.             m_LPos=m_Route[nRouteIdx-1].Vtx;
  1043.             CalcRoadTarget(nRouteIdx,Position);
  1044.             if(int nTurnId=InSharpTurn(nRouteIdx))
  1045.             {
  1046.                 m_Route[nRouteIdx].m_nTurnId=nTurnId-1;
  1047.                 CalcSharpTurnTarget(nRouteIdx,nRouteIdx,Position);
  1048.             }
  1049.         }
  1050.     }
  1051.  
  1052.     // By Default All position calculated by the above on on the road
  1053.     m_Route[nRouteIdx].m_nOnRoad=1;
  1054.  
  1055.     // Now that we know where we want to go based on the road, lets check if there are
  1056.     // any obstacles in the way, and if the target is within the road boundaries.
  1057.     short *pnSRd=&m_Route[nRouteIdx-1].m_nSRd;
  1058.     short *pnSVIdx=&m_Route[nRouteIdx-1].m_nSVIdx;
  1059.     short *pnCRd=&m_Route[nRouteIdx].m_nSRd;
  1060.     short *pnCVIdx=&m_Route[nRouteIdx].m_nSVIdx;
  1061.  
  1062.     int nType;
  1063.     Vector3 ObsPos;
  1064.     aiObstacle *pObstacle;
  1065.     pObstacle=IsTargetBlocked(m_Route[nRouteIdx-1].Vtx,m_Route[nRouteIdx].Vtx,*pnSVIdx,
  1066.                               *pnSRd,*pnCVIdx+3,*pnCRd,VehicleLength()*2.f,&nType);
  1067.     if(pObstacle)
  1068.     {
  1069.         pObstacle->Position(ObsPos);
  1070.         if((nType!=kOppObstacle || (nType==kOppObstacle && m_nWayPtIdx>2 && m_Route[nRouteIdx-1].m_nType!=kTurnTarget)) &&
  1071.            aiDist2(m_Route[nRouteIdx-1].Vtx,ObsPos)<m_fRoutePlanDist*m_fRoutePlanDist)
  1072.         {
  1073.             short Locations[10], TargetTypes[10];
  1074.             Vector3 Targets[10];
  1075.             aiObstacle *pAltObstacles[10];
  1076.  
  1077.             int bUseSidewalk=true;
  1078.             int nNumTargets=CalcObstacleAvoidPoints(pObstacle,nRouteIdx,bUseSidewalk,Targets,
  1079.                                                     pAltObstacles,Locations,TargetTypes);
  1080.             for(int ctr=0;ctr<nNumTargets;ctr++)
  1081.                 SaveTarget(nRouteIdx,Targets[ctr],pAltObstacles[ctr],Locations[ctr],nType,TargetTypes[ctr]);
  1082.         }
  1083.         else
  1084.         {
  1085.             ContinueCheck(nRouteIdx);
  1086.         }
  1087.     }
  1088.     else
  1089.     {
  1090.         ContinueCheck(nRouteIdx);
  1091.     }
  1092. }
  1093.  
  1094. // ASSUMPTION:
  1095. // Will always return at least one route when no acceptable targets exist, 
  1096. // which is the current road target .
  1097. int aiNavigation::CalcObstacleAvoidPoints(aiObstacle *pObstacle,int nRouteIdx,int bUseSidewalk,
  1098.                                                 Vector3* pTargets,aiObstacle **ppAltObstacles,
  1099.                                                 short* pLocations,short* pTargetTypes)
  1100. {
  1101.     int nNumTargets=0;
  1102.     if(m_Route[nRouteIdx-1].m_nType!=kTurnTarget)
  1103.     {
  1104.         float fZ;
  1105.         Vector3 p, LTarget, RTarget;
  1106.  
  1107.         short *pnSRd=&m_Route[nRouteIdx-1].m_nSRd;
  1108.         short *pnSVIdx=&m_Route[nRouteIdx-1].m_nSVIdx;
  1109.  
  1110.         if(nRouteIdx==1)
  1111.             pObstacle->PreAvoid(m_Route[nRouteIdx-1].Vtx,-m_Car.GetMatrix().ZDir,
  1112.                                 m_fRSideDist+2.f,LTarget,RTarget);
  1113.         else
  1114.             pObstacle->PreAvoid(m_Route[nRouteIdx-1].Vtx,m_Route[nRouteIdx-1].Vtx-m_Route[nRouteIdx-2].Vtx,
  1115.                                 m_fRSideDist+2.f,LTarget,RTarget);
  1116.         
  1117.         // Now calc the left avoid points
  1118.         p=LTarget-m_Route[nRouteIdx-1].Vtx;
  1119.         if(nRouteIdx==1)
  1120.         {
  1121.             fZ=aiDot(p,-m_Car.GetMatrix().ZDir);
  1122.         }
  1123.         else
  1124.         {
  1125.             if(m_bDir[*pnSRd])
  1126.                 fZ=aiDot(p,(-(m_pRoads[*pnSRd]->VertZDir(*pnSVIdx))));
  1127.             else
  1128.                 fZ=aiDot(p,m_pRoads[*pnSRd]->VertZDir(m_pRoads[*pnSRd]->NumVerts()-*pnSVIdx-1));
  1129.         }
  1130.  
  1131.         if(fZ>=0.f)
  1132.         {
  1133.             int nNumTries=0;
  1134.             EnumTargets(LTarget,pObstacle,nRouteIdx,*pnSRd,*pnSVIdx,-1,bUseSidewalk,nNumTries,
  1135.                         pTargets,ppAltObstacles,pLocations,pTargetTypes,&nNumTargets);
  1136.         }
  1137.  
  1138.         // Now calc the right avoid points
  1139.         p=RTarget-m_Route[nRouteIdx-1].Vtx;
  1140.         if(nRouteIdx==1)
  1141.         {
  1142.             fZ=aiDot(p,-m_Car.GetMatrix().ZDir);
  1143.         }
  1144.         else
  1145.         {
  1146.             if(m_bDir[*pnSRd])
  1147.                 fZ=aiDot(p,(-(m_pRoads[*pnSRd]->VertZDir(m_pRoads[*pnSRd]->NumVerts()-*pnSVIdx-1))));
  1148.             else
  1149.                 fZ=aiDot(p,m_pRoads[*pnSRd]->VertZDir(*pnSVIdx));
  1150.         }
  1151.  
  1152.         if(fZ>=0.f)
  1153.         {
  1154.             int nNumTries=0;
  1155.             EnumTargets(RTarget,pObstacle,nRouteIdx,*pnSRd,*pnSVIdx,1,bUseSidewalk,nNumTries,
  1156.                         pTargets,ppAltObstacles,pLocations,pTargetTypes,&nNumTargets);
  1157.         }
  1158.  
  1159.         // Bailout: When no routes are found, ignore the obstacle and plan on braking.
  1160.         if(!nNumTargets)
  1161.         {
  1162.             pTargets[nNumTargets]=m_Route[nRouteIdx].Vtx;
  1163.             *ppAltObstacles=pObstacle;
  1164.             pTargetTypes[nNumTargets]=kBlockedTarget;
  1165.             nNumTargets=1;
  1166.         }
  1167.  
  1168.         return nNumTargets;
  1169.     }
  1170.  
  1171.     pTargets[nNumTargets]=m_Route[nRouteIdx].Vtx;
  1172.     pTargetTypes[nNumTargets]=kTurnTarget;
  1173.     *ppAltObstacles=pObstacle;
  1174.     nNumTargets=1;
  1175.  
  1176.     return nNumTargets;
  1177. }
  1178.  
  1179. void aiNavigation::EnumTargets(Vector3& OrigTarget,aiObstacle *pOrigObstacle,int nRouteIdx,
  1180.                                      int nSRd,int nSVIdx,int nSide,int bUseSidewalk,int nNumTries,
  1181.                                      Vector3* pTargets,aiObstacle **ppAltObstacles,short* pLocations,
  1182.                                      short* pTargetTypes,int *pnNumTargets)
  1183. {
  1184.     nNumTries++;
  1185.     if(nNumTries==_nMaxTrys)
  1186.         return;
  1187.  
  1188.     int nCRdVtx;
  1189.     int nCRdIdx=pOrigObstacle->CurrentRoadIdx(m_pRoads,m_bDir,&nCRdVtx);
  1190.     if(nCRdIdx<0 || !m_pRoads[nCRdIdx])
  1191.         return;
  1192.  
  1193.     float fSideDist;
  1194.     int nOnRoad=m_pRoads[nCRdIdx]->IsPosOnRoad(OrigTarget,m_fRSideDist,&fSideDist);
  1195.     if(nOnRoad==1 || (bUseSidewalk && nOnRoad==2))
  1196.     {
  1197.         // Finally check if any other ambients block the route
  1198.         int nType;
  1199.         aiObstacle *pNewObstacle;
  1200.         pNewObstacle=IsTargetBlocked(m_Route[nRouteIdx-1].Vtx,OrigTarget,nSVIdx,nSRd,
  1201.                                      nCRdVtx+2,nCRdIdx,VehicleLength()*2.f,&nType);
  1202.         if(pNewObstacle && nType!=kOppObstacle)
  1203.         {
  1204.             Vector3 Obs1Pos, Obs2Pos, LTarget, RTarget;
  1205.             if(nRouteIdx==1)
  1206.                 pNewObstacle->PreAvoid(m_Route[nRouteIdx-1].Vtx,-m_Car.GetMatrix().ZDir,
  1207.                                        m_fRSideDist+2.f,LTarget,RTarget);
  1208.             else
  1209.                 pNewObstacle->PreAvoid(m_Route[nRouteIdx-1].Vtx,m_Route[nRouteIdx-1].Vtx-m_Route[nRouteIdx-2].Vtx,
  1210.                                        m_fRSideDist+2.f,LTarget,RTarget);
  1211.  
  1212.             // Check the distance between the two obstacles
  1213.             pOrigObstacle->Position(Obs1Pos);
  1214.             pNewObstacle->Position(Obs2Pos);
  1215.             float fSepDist=aiDist2(Obs1Pos,Obs2Pos);
  1216.             float fOrigObsDist=aiDist2(Obs1Pos,m_Car.GetMatrix().Pos);
  1217.             float fNewObsDist=aiDist2(Obs2Pos,m_Car.GetMatrix().Pos);
  1218.             if(fSepDist>225.f && fNewObsDist<fOrigObsDist)
  1219.             {
  1220.                 float fZ;
  1221.                 float fCheckDist=m_fRoutePlanDist*m_fRoutePlanDist;
  1222.                 if(nSide==-1)
  1223.                 {
  1224.                     Vector3 p=RTarget-m_Route[nRouteIdx-1].Vtx;
  1225.                     if(m_bDir[nSRd])
  1226.                     {
  1227.                         fZ=aiDot(p,(-(m_pRoads[nSRd]->VertZDir(nSVIdx))));
  1228.                     }
  1229.                     else
  1230.                     {
  1231.                         int nNumVerts=m_pRoads[nSRd]->NumVerts();
  1232.                         fZ=aiDot(p,m_pRoads[nSRd]->VertZDir(nNumVerts-nSVIdx-1));
  1233.                     }
  1234.  
  1235.                     if(fZ>=0.f &&
  1236.                        aiDist2(m_Route[nRouteIdx-1].Vtx,Obs2Pos)<fCheckDist &&
  1237.                        nType==kAmbObstacle)
  1238.                     {
  1239.                         ppAltObstacles[*pnNumTargets]=pNewObstacle;
  1240.                         pTargets[*pnNumTargets]=RTarget;
  1241.                         pLocations[*pnNumTargets]=nOnRoad;
  1242.                         pTargetTypes[*pnNumTargets]=kObstacleTarget;
  1243.                         (*pnNumTargets)++;
  1244.                     }
  1245.                 }
  1246.                 else
  1247.                 {
  1248.                     Vector3 p=LTarget-m_Route[nRouteIdx-1].Vtx;
  1249.                     if(m_bDir[nSRd])
  1250.                     {
  1251.                         fZ=aiDot(p,(-(m_pRoads[nSRd]->VertZDir(nSVIdx))));
  1252.                     }
  1253.                     else
  1254.                     {
  1255.                         int nNumVerts=m_pRoads[nSRd]->NumVerts();
  1256.                         fZ=aiDot(p,m_pRoads[nSRd]->VertZDir(nNumVerts-nSVIdx-1));
  1257.                     }
  1258.  
  1259.                     if(fZ>=0.f &&
  1260.                        aiDist2(m_Route[nRouteIdx-1].Vtx,Obs2Pos)<fCheckDist &&
  1261.                        nType==kAmbObstacle)
  1262.                     {
  1263.                         ppAltObstacles[*pnNumTargets]=pNewObstacle;
  1264.                         pTargets[*pnNumTargets]=LTarget;
  1265.                         pLocations[*pnNumTargets]=nOnRoad;
  1266.                         pTargetTypes[*pnNumTargets]=kObstacleTarget;
  1267.                         (*pnNumTargets)++;
  1268.                     }
  1269.                 }
  1270.             }
  1271.  
  1272.             // Now calc the specified avoid points.
  1273.             float fZ;
  1274.             if(nSide==-1)
  1275.             {
  1276.                 Vector3 p=LTarget-m_Route[nRouteIdx-1].Vtx;
  1277.                 if(m_bDir[nSRd])
  1278.                 {
  1279.                     fZ=aiDot(p,(-(m_pRoads[nSRd]->VertZDir(nSVIdx))));
  1280.                 }
  1281.                 else
  1282.                 {
  1283.                     int nNumVerts=m_pRoads[nSRd]->NumVerts();
  1284.                     fZ=aiDot(p,m_pRoads[nSRd]->VertZDir(nNumVerts-nSVIdx-1));
  1285.                 }
  1286.  
  1287.                 if(fZ>=0.f)
  1288.                 {
  1289.                     EnumTargets(LTarget,pNewObstacle,nRouteIdx,nSRd,nSVIdx,-1,bUseSidewalk,nNumTries,
  1290.                                 pTargets,ppAltObstacles,pLocations,pTargetTypes,pnNumTargets);
  1291.                 }
  1292.             }
  1293.             else
  1294.             {
  1295.                 Vector3 p=RTarget-m_Route[nRouteIdx-1].Vtx;
  1296.                 if(m_bDir[nSRd])
  1297.                 {
  1298.                     fZ=aiDot(p,(-(m_pRoads[nSRd]->VertZDir(nSVIdx))));
  1299.                 }
  1300.                 else
  1301.                 {
  1302.                     int nNumVerts=m_pRoads[nSRd]->NumVerts();
  1303.                     fZ=aiDot(p,m_pRoads[nSRd]->VertZDir(nNumVerts-nSVIdx-1));
  1304.                 }
  1305.  
  1306.                 if(fZ>=0.f)
  1307.                 {
  1308.                     EnumTargets(RTarget,pNewObstacle,nRouteIdx,nSRd,nSVIdx,1,bUseSidewalk,nNumTries,
  1309.                                 pTargets,ppAltObstacles,pLocations,pTargetTypes,pnNumTargets);
  1310.                 }
  1311.             }
  1312.         }
  1313.         else
  1314.         {
  1315.             // Save off this target.
  1316.             ppAltObstacles[*pnNumTargets]=pOrigObstacle;
  1317.             pTargets[*pnNumTargets]=OrigTarget;
  1318.             pLocations[*pnNumTargets]=nOnRoad;
  1319.             pTargetTypes[*pnNumTargets]=kObstacleTarget;
  1320.             (*pnNumTargets)++;
  1321.         }
  1322.     }
  1323. }
  1324.  
  1325. int aiNavigation::CalcSharpTurnTarget(int& nRouteIdx,int nLastIdx,Vector3& Position)
  1326. {
  1327.     Vector3 XDir, ZDir;
  1328.     if(m_bDir[m_Route[nLastIdx].m_nTurnId])
  1329.     {
  1330.         XDir=m_pRoads[m_Route[nLastIdx].m_nTurnId]->VertXDir(m_pRoads[m_Route[nLastIdx].m_nTurnId]->NumVerts()-1);
  1331.         ZDir=m_pRoads[m_Route[nLastIdx].m_nTurnId]->VertZDir(m_pRoads[m_Route[nLastIdx].m_nTurnId]->NumVerts()-1);
  1332.     }
  1333.     else
  1334.     {
  1335.         XDir=-m_pRoads[m_Route[nLastIdx].m_nTurnId]->VertXDir(0);
  1336.         ZDir=-m_pRoads[m_Route[nLastIdx].m_nTurnId]->VertZDir(0);
  1337.     }
  1338.  
  1339.     Vector3 StartPt=m_TurnCenterPt[m_Route[nLastIdx].m_nTurnId]+XDir*m_fTurn[m_Route[nLastIdx].m_nTurnId]*m_fRadius[m_Route[nLastIdx].m_nTurnId];
  1340.  
  1341.     Vector3 MyDir=m_Route[nRouteIdx-1].Vtx-m_TurnCenterPt[m_Route[nLastIdx].m_nTurnId];
  1342.     Vector3 StartPerp;
  1343.     StartPerp.Set(-m_TurnStartDir[m_Route[nLastIdx].m_nTurnId].z,
  1344.                   m_TurnStartDir[m_Route[nLastIdx].m_nTurnId].y,
  1345.                   m_TurnStartDir[m_Route[nLastIdx].m_nTurnId].x);
  1346.     float fX=aiDot(MyDir,StartPerp);
  1347.     float fZ=aiDot(MyDir,m_TurnStartDir[m_Route[nLastIdx].m_nTurnId]);
  1348.     float fMyAngle=atan2f(fX,fZ);
  1349.     m_Route[nRouteIdx].m_nTurnId=m_Route[nLastIdx].m_nTurnId;
  1350.  
  1351.     if(m_fTurn[m_Route[nLastIdx].m_nTurnId]<0.f)
  1352.         fMyAngle*=-1.f;
  1353.     
  1354.     if(fMyAngle<-0.1f)
  1355.     {
  1356.         m_Route[nRouteIdx].Vtx=StartPt;
  1357.         SaveTurnTarget(nRouteIdx,1);
  1358.         m_Route[nRouteIdx].m_nSRd=m_Route[nRouteIdx].m_nTurnId;
  1359.         m_Route[nRouteIdx].m_nSVIdx=m_pRoads[m_Route[nRouteIdx].m_nSRd]->NumVerts()-1;
  1360.         m_Route[nRouteIdx].m_nType=kRoadTarget;
  1361.         return 0;
  1362.     }
  1363.     else
  1364.     {
  1365.         float fCircumfrence=m_fRadius[m_Route[nLastIdx].m_nTurnId]*m_fRdAngle[m_Route[nLastIdx].m_nTurnId]*m_fTurn[m_Route[nLastIdx].m_nTurnId];
  1366.         int nNumSteps=(int)(fCircumfrence*.1f);
  1367.         nNumSteps+=(nNumSteps%2);    // Make always even
  1368.         if(nNumSteps<2)
  1369.             nNumSteps=2;
  1370.  
  1371.         float fStepAngle=(m_fRdAngle[m_Route[nLastIdx].m_nTurnId]*m_fTurn[m_Route[nLastIdx].m_nTurnId])/nNumSteps;
  1372.         for(int i=1;i<nNumSteps+1;i++)
  1373.         {
  1374.             if(fMyAngle<(fStepAngle*i-.5f*fStepAngle))
  1375.             {
  1376.                 float fX=m_fRadius[m_Route[nLastIdx].m_nTurnId]*cosf(fStepAngle*i);
  1377.                 float fZ=m_fRadius[m_Route[nLastIdx].m_nTurnId]*sinf(fStepAngle*i);
  1378.                 Vector3 Target=m_TurnCenterPt[m_Route[nLastIdx].m_nTurnId]-ZDir*fZ+
  1379.                                XDir*m_fTurn[m_Route[nLastIdx].m_nTurnId]*fX;
  1380.                 Vector3 p=Target-m_Route[nRouteIdx-1].Vtx;
  1381.                 float fDist=(float)fabsf(aiDot(p,m_Car.GetMatrix().XDir));
  1382.  
  1383.                 m_Route[nRouteIdx].Vtx=Target;
  1384.  
  1385.                 SaveTurnTarget(nRouteIdx,0);
  1386.                 if(i*fStepAngle<m_fRdAngle[m_Route[nLastIdx].m_nTurnId]*m_fTurn[m_Route[nLastIdx].m_nTurnId]*.5f)
  1387.                 {
  1388.                     m_Route[nRouteIdx].m_nSRd=m_Route[nLastIdx].m_nTurnId;
  1389.                     m_Route[nRouteIdx].m_nSVIdx=m_pRoads[m_Route[nRouteIdx].m_nSRd]->NumVerts()-1;
  1390.                 }
  1391.                 else
  1392.                 {
  1393.                     m_Route[nRouteIdx].m_nSRd=m_Route[nLastIdx].m_nTurnId+1;
  1394.                     int nRdId=m_Route[nRouteIdx].m_nSRd;
  1395.                     m_Route[nRouteIdx].m_nSVIdx=m_pRoads[nRdId]->RoadVertice(m_Route[nRouteIdx].Vtx,m_bDir[nRdId],-1);
  1396.                     if(m_Route[nRouteIdx].m_nSVIdx>=m_pRoads[nRdId]->NumVerts())
  1397.                     {
  1398.                         m_Route[nRouteIdx].m_nSRd++;
  1399.                         m_Route[nRouteIdx].m_nSVIdx=0;
  1400.                     }
  1401.         
  1402.                     if(nRdId>=m_nRdIdx)
  1403.                         m_bFinished=true;
  1404.                 }
  1405.  
  1406.                 m_Route[nRouteIdx].m_nTurnId=m_Route[nLastIdx].m_nTurnId;
  1407.  
  1408.                 float fTmpSideDist;
  1409.                 if(i*2>nNumSteps && 
  1410.                    m_pRoads[m_Route[nRouteIdx].m_nTurnId+1]->IsPosOnRoad(m_Route[nRouteIdx].Vtx,0.f,&fTmpSideDist)<3)
  1411.                 {
  1412.                     m_Route[nRouteIdx].m_nTurnId++;
  1413.                     m_LPos=m_Route[nRouteIdx].Vtx;
  1414.                     break;
  1415.                 }
  1416.  
  1417.                 if(i<nNumSteps)
  1418.                     nRouteIdx++;
  1419.             }
  1420.         }
  1421.     }
  1422.  
  1423.     if(m_pRoads[m_Route[nRouteIdx].m_nSRd])
  1424.         m_Route[nRouteIdx].m_nSVIdx=aiClamp(m_Route[nRouteIdx].m_nSVIdx,0,
  1425.                                             m_pRoads[m_Route[nRouteIdx].m_nSRd]->NumVerts()-1);
  1426.     return 1;
  1427. }
  1428.  
  1429. void aiNavigation::SaveTurnTarget(int nRouteIdx,int bDoAngleCalc)
  1430. {
  1431.     m_Route[nRouteIdx].m_nType=kTurnTarget;
  1432.     m_Route[nRouteIdx].Vtx.y+=1.f;
  1433.     m_Route[nRouteIdx].m_fCumDist=m_Route[nRouteIdx-1].m_fCumDist+
  1434.                                   aiDist(m_Route[nRouteIdx-1].Vtx,m_Route[nRouteIdx].Vtx);
  1435.     if(bDoAngleCalc)
  1436.     {
  1437.         if(nRouteIdx==1)
  1438.         {
  1439.             Vector3 TargetDir=m_Route[nRouteIdx].Vtx-m_Car.GetMatrix().Pos;
  1440.             m_Route[nRouteIdx].m_fCumAngle=(-m_Car.GetMatrix().ZDir).Angle(TargetDir);
  1441.         }
  1442.         else
  1443.         {
  1444.             Vector3 TargetDir=m_Route[nRouteIdx].Vtx-m_Route[nRouteIdx-1].Vtx;
  1445.             Vector3 PTargetDir=m_Route[nRouteIdx-1].Vtx-m_Route[nRouteIdx-2].Vtx;
  1446.             m_Route[nRouteIdx].m_fCumAngle=m_Route[nRouteIdx-1].m_fCumAngle+
  1447.                                            PTargetDir.Angle(TargetDir);
  1448.         }
  1449.     }
  1450. }
  1451.  
  1452. void aiNavigation::InitRoadTurns()
  1453. {
  1454.     // Calc the road angles
  1455.     for(int i=0;i<2;i++)
  1456.     {
  1457.         if(m_pRoads[i] && m_pRoads[i+1])
  1458.         {
  1459.             float fX, fZ;
  1460.             Vector3 TargetDir;
  1461.             int nNumVerts=m_pRoads[i]->NumVerts();
  1462.             int nNNumVerts=m_pRoads[i+1]->NumVerts();
  1463.             if(m_bDir[i+1])
  1464.                 TargetDir=m_pRoads[i+1]->CenterVertice(1)-m_pRoads[i+1]->CenterVertice(0);
  1465.             else
  1466.                 TargetDir=m_pRoads[i+1]->CenterVertice(nNNumVerts-2)-m_pRoads[i+1]->CenterVertice(nNNumVerts-1);
  1467.  
  1468.             if(m_bDir[i])
  1469.             {
  1470.                 fX=aiDot(TargetDir,(-(m_pRoads[i]->VertXDir(nNumVerts-1))));
  1471.                 fZ=aiDot(TargetDir,(-(m_pRoads[i]->VertZDir(nNumVerts-1))));
  1472.             }
  1473.             else
  1474.             {
  1475.                 fX=aiDot(TargetDir,m_pRoads[i]->VertXDir(0));
  1476.                 fZ=aiDot(TargetDir,m_pRoads[i]->VertZDir(0));
  1477.             }
  1478.             m_fRdAngle[i]=atan2f(fX,fZ);
  1479.  
  1480.             // Calc the direction of the turn
  1481.             if(m_fRdAngle[i]>=_fTurnThreshold)
  1482.                 m_fTurn[i]=1.f;
  1483.             else if(m_fRdAngle[i]<=-_fTurnThreshold)
  1484.                 m_fTurn[i]=-1.f;
  1485.             else
  1486.                 m_fTurn[i]=0.f;
  1487.         }
  1488.         else
  1489.         {
  1490.             m_fRdAngle[i]=0.f;
  1491.             m_fTurn[i]=0.f;
  1492.         }
  1493.     }
  1494. }
  1495.  
  1496. void aiNavigation::CalcRoadTurns()
  1497. {
  1498.     for(int i=0;i<2;i++)
  1499.     {
  1500.         if(m_fTurn[i] && m_pRoads[i] && m_pRoads[i+1])
  1501.         {
  1502.             // Calc the intersection point of the current and next roads.
  1503.             float fWidth=CalcTurnIntersection(i);
  1504.         
  1505.             float fTheta=3.14f-fabsf(m_fRdAngle[i]);
  1506.             float fHalfTheta=fTheta*.5f;
  1507.  
  1508.             m_fRadius[i]=fWidth/(1.f-sinf(fHalfTheta));
  1509.             m_fTurnSetback[i]=m_fRadius[i]*cosf(fHalfTheta);
  1510.  
  1511.             if(m_bDir[i])
  1512.                 m_TurnCenterPt[i]=m_IntersectionPt[i]+
  1513.                                   m_pRoads[i]->VertZDir(m_pRoads[i]->NumVerts()-1)*m_fTurnSetback[i]-
  1514.                                   m_pRoads[i]->VertXDir(m_pRoads[i]->NumVerts()-1)*m_fTurn[i]*(m_fRadius[i]-fWidth);
  1515.             else
  1516.                 m_TurnCenterPt[i]=m_IntersectionPt[i]+
  1517.                                   -m_pRoads[i]->VertZDir(0)*m_fTurnSetback[i]-
  1518.                                   -m_pRoads[i]->VertXDir(0)*m_fTurn[i]*(m_fRadius[i]-fWidth);
  1519.  
  1520.             Vector3 XDir, ZDir;
  1521.             if(m_bDir[i])
  1522.             {
  1523.                 XDir=m_pRoads[i]->VertXDir(m_pRoads[i]->NumVerts()-1);
  1524.                 ZDir=m_pRoads[i]->VertZDir(m_pRoads[i]->NumVerts()-1);
  1525.             }
  1526.             else
  1527.             {
  1528.                 XDir=-m_pRoads[i]->VertXDir(0);
  1529.                 ZDir=-m_pRoads[i]->VertZDir(0);
  1530.             }
  1531.  
  1532.             Vector3 StartPt=m_TurnCenterPt[i]+XDir*m_fTurn[i]*m_fRadius[i];
  1533.             m_TurnStartDir[i]=StartPt-m_TurnCenterPt[i];
  1534.             m_TurnStartDir[i].y=0.f;
  1535.             m_TurnStartDir[i].Normalize();
  1536.  
  1537.             float fX=m_fRadius[i]*cosf(m_fRdAngle[i]*m_fTurn[i]);
  1538.             float fZ=m_fRadius[i]*sinf(m_fRdAngle[i]*m_fTurn[i]);
  1539.             Vector3 EndPt=m_TurnCenterPt[i]-ZDir*fZ+XDir*m_fTurn[i]*fX;
  1540.             m_TurnEndDir[i]=EndPt-m_TurnCenterPt[i];
  1541.             m_TurnEndDir[i].y=0.f;
  1542.             m_TurnEndDir[i].Normalize();
  1543.         }
  1544.     }
  1545. }
  1546.  
  1547. float aiNavigation::CalcTurnIntersection(int nWhichTurn)
  1548. {
  1549.     int nNumVerts=m_pRoads[nWhichTurn]->NumVerts();
  1550.     int nNNumVerts=m_pRoads[nWhichTurn+1]->NumVerts();
  1551.  
  1552.     // Calc the turn offsets. This could be moved away a little from the sidewalk
  1553.     float fCRdOffset=0.f;
  1554.     float fNRdOffset=0.f;
  1555.  
  1556.     float t;
  1557.     Vector3 P0, P1, Q0, Q1;
  1558.     if(m_fTurn[nWhichTurn]>0.f)    // right turn
  1559.     {
  1560.         int nVerts=0;
  1561.         if(m_bDir[nWhichTurn])
  1562.         {
  1563.             P0=m_pRoads[nWhichTurn]->RBoundary(nNumVerts-1+nVerts)+m_pRoads[nWhichTurn]->VertXDir(nNumVerts-1)*fCRdOffset; 
  1564.             P1=-(m_pRoads[nWhichTurn]->VertZDir(nNumVerts-1));
  1565.         }
  1566.         else
  1567.         {
  1568.             P0=m_pRoads[nWhichTurn]->LBoundary(0+nVerts)-m_pRoads[nWhichTurn]->VertXDir(0)*fCRdOffset; 
  1569.             P1=-(m_pRoads[nWhichTurn]->VertZDir(0));
  1570.         }
  1571.  
  1572.         if(m_bDir[nWhichTurn+1])
  1573.         {
  1574.             Q0=m_pRoads[nWhichTurn+1]->RBoundary(0+nVerts)+m_pRoads[nWhichTurn+1]->VertXDir(0)*fNRdOffset; 
  1575.             Q1=m_pRoads[nWhichTurn+1]->VertZDir(0);
  1576.             m_IntersectionPt[nWhichTurn].y=m_pRoads[nWhichTurn+1]->RBoundary(0).y;
  1577.         }
  1578.         else
  1579.         {
  1580.             Q0=m_pRoads[nWhichTurn+1]->LBoundary(nNNumVerts-1+nVerts)-m_pRoads[nWhichTurn+1]->VertXDir(nNNumVerts-1)*fNRdOffset; 
  1581.             Q1=m_pRoads[nWhichTurn+1]->VertZDir(nNNumVerts-1);
  1582.             m_IntersectionPt[nWhichTurn].y=m_pRoads[nWhichTurn+1]->LBoundary(nNNumVerts-1).y;
  1583.         }
  1584.     }
  1585.     else
  1586.     {
  1587.         int nVerts=0;
  1588.         if(m_bDir[nWhichTurn])
  1589.         {
  1590.             P0=m_pRoads[nWhichTurn]->LBoundary(nNumVerts-1+nVerts)-m_pRoads[nWhichTurn]->VertXDir(nNumVerts-1)*fCRdOffset;
  1591.             P1=-(m_pRoads[nWhichTurn]->VertZDir(nNumVerts-1));
  1592.         }
  1593.         else
  1594.         {
  1595.             P0=m_pRoads[nWhichTurn]->RBoundary(0+nVerts)+m_pRoads[nWhichTurn]->VertXDir(0)*fCRdOffset;
  1596.             P1=-(m_pRoads[nWhichTurn]->VertZDir(0));
  1597.         }
  1598.  
  1599.         if(m_bDir[nWhichTurn+1])
  1600.         {
  1601.             Q0=m_pRoads[nWhichTurn+1]->LBoundary(0+nVerts)-m_pRoads[nWhichTurn+1]->VertXDir(0)*fNRdOffset;
  1602.             Q1=-(m_pRoads[nWhichTurn+1]->VertZDir(0));
  1603.             m_IntersectionPt[nWhichTurn].y=m_pRoads[nWhichTurn+1]->LBoundary(0).y;
  1604.         }
  1605.         else
  1606.         {
  1607.             Q0=m_pRoads[nWhichTurn+1]->RBoundary(nNNumVerts-1+nVerts)+m_pRoads[nWhichTurn+1]->VertXDir(nNNumVerts-1)*fNRdOffset;
  1608.             Q1=m_pRoads[nWhichTurn+1]->VertZDir(nNNumVerts-1);
  1609.             m_IntersectionPt[nWhichTurn].y=m_pRoads[nWhichTurn+1]->RBoundary(nNNumVerts-1).y;
  1610.         }
  1611.     }
  1612.  
  1613.     t=(P1.x*Q0.z-P1.x*P0.z-P1.z*Q0.x+P1.z*P0.x)/(P1.z*Q1.x-P1.x*Q1.z);
  1614.     m_IntersectionPt[nWhichTurn].x=Q0.x+t*Q1.x;
  1615.     m_IntersectionPt[nWhichTurn].z=Q0.z+t*Q1.z;
  1616.  
  1617.     // Add 1.f to the width in order to make the turn grow as the opp
  1618.     // gets closer to the turn, but only if more than 10m to the turn.
  1619.     float fOffset=0.f;
  1620.     float fDist2=(m_fTurnSetback[nWhichTurn]+12.f)*(m_fTurnSetback[nWhichTurn]+12.f);
  1621.     if(aiDist2(m_LPos,m_IntersectionPt[nWhichTurn])>fDist2)
  1622.         fOffset=1.f;
  1623.  
  1624.     float fWidth;
  1625.     Vector3 p=m_LPos-m_IntersectionPt[nWhichTurn];
  1626.     if(m_bDir[nWhichTurn]==1)
  1627.         fWidth=m_fTurn[nWhichTurn]*aiDot(p,m_pRoads[nWhichTurn]->VertXDir(m_pRoads[nWhichTurn]->NumVerts()-1))+fOffset;
  1628.     else
  1629.         fWidth=m_fTurn[nWhichTurn]*aiDot(p,-m_pRoads[nWhichTurn]->VertXDir(0))+fOffset;
  1630.  
  1631.     return fWidth;
  1632. }
  1633.  
  1634. int aiNavigation::InSharpTurn(int nRouteIdx)
  1635. {
  1636.     int i;
  1637.     Vector3 MyDir;
  1638.  
  1639.     for(i=m_Route[nRouteIdx].m_nSRd;i<3;i++)
  1640.     {
  1641.         // Test the intersections
  1642.         if(i==0 || i==1 && m_Route[nRouteIdx].m_nTurnId==1 ||
  1643.           (i==1 && -_fMaxStraightAngle<m_fRdAngle[0] && m_fRdAngle[0]<_fMaxStraightAngle))
  1644.         {
  1645.             float fRadius2=(m_fRadius[i]+15.f)*(m_fRadius[i]+15.f);
  1646.             if(m_fTurn[i] && m_pRoads[i] && m_pRoads[i+1] && aiDist2(m_Route[nRouteIdx].Vtx,m_TurnCenterPt[i])<fRadius2)
  1647.             {
  1648.                 MyDir=m_Route[nRouteIdx].Vtx-m_TurnCenterPt[i];
  1649.                 
  1650.                 Vector3 StartPerp;
  1651.                 StartPerp.Set(-m_TurnStartDir[i].z,m_TurnStartDir[i].y,m_TurnStartDir[i].x);
  1652.                 float fSX=aiDot(MyDir,StartPerp*m_fTurn[i]);
  1653.                 
  1654.                 Vector3 EndPerp;
  1655.                 EndPerp.Set(m_TurnEndDir[i].z,m_TurnEndDir[i].y,-m_TurnEndDir[i].x);
  1656.                 float fEX=aiDot(MyDir,EndPerp*m_fTurn[i]);
  1657.                 if(fSX>=-0.01f && fEX>=0.01f)
  1658.                     return i+1;
  1659.             }
  1660.         }
  1661.     }
  1662.  
  1663.     return 0;
  1664. }
  1665.  
  1666. void aiNavigation::SaveTarget(int nRouteIdx,Vector3& Target,aiObstacle *pObstacle,short& Location,
  1667.                                     int nObsType,short& TargetType)
  1668. {
  1669.     int nNVIdx;
  1670.     int nCRd=pObstacle->CurrentRoadIdx(m_pRoads,m_bDir,&nNVIdx);
  1671.     if(!m_pRoads[nCRd])
  1672.     {
  1673.         nCRd--;
  1674.         if(nCRd<0)
  1675.             return;
  1676.         nNVIdx=m_pRoads[nCRd]->NumVerts()-1;
  1677.     }
  1678.     nNVIdx=aiClamp(nNVIdx,0,m_pRoads[nCRd]->NumVerts()-1);
  1679.  
  1680.     m_Route[nRouteIdx].m_nType=TargetType;
  1681.     m_Route[nRouteIdx].m_nObsType=nObsType;
  1682.     m_Route[nRouteIdx].m_pObstacle=pObstacle;
  1683.     m_Route[nRouteIdx].m_nOnRoad=Location;
  1684.     m_Route[nRouteIdx].Vtx=Target;
  1685.     m_Route[nRouteIdx].Vtx.y+=1.f;
  1686.  
  1687.     m_Route[nRouteIdx].m_fCumDist=m_Route[nRouteIdx-1].m_fCumDist+
  1688.                                   aiDist(m_Route[nRouteIdx-1].Vtx,m_Route[nRouteIdx].Vtx);
  1689.     if(nRouteIdx==1)
  1690.     {
  1691.         Vector3 TargetDir=m_Route[nRouteIdx].Vtx-m_Car.GetMatrix().Pos;
  1692.         m_Route[nRouteIdx].m_fCumAngle=(-m_Car.GetMatrix().ZDir).Angle(TargetDir);
  1693.     }
  1694.     else
  1695.     {
  1696.         Vector3 TargetDir=m_Route[nRouteIdx].Vtx-m_Route[nRouteIdx-1].Vtx;
  1697.         Vector3 PTargetDir=m_Route[nRouteIdx-1].Vtx-m_Route[nRouteIdx-2].Vtx;
  1698.         m_Route[nRouteIdx].m_fCumAngle=m_Route[nRouteIdx-1].m_fCumAngle+
  1699.                                        PTargetDir.Angle(TargetDir);
  1700.     }
  1701.     m_Route[nRouteIdx].m_nSVIdx=nNVIdx;
  1702.     m_Route[nRouteIdx].m_nSRd=nCRd;
  1703.     ContinueCheck(nRouteIdx);
  1704. }
  1705.  
  1706. void aiNavigation::ContinueCheck(int nRouteIdx)
  1707. {
  1708.     // Test if we're at the end of the route and are now ready to navigate to the destination
  1709.     // point.
  1710.     if(m_Route[nRouteIdx].m_fCumDist<m_fRoutePlanDist && 
  1711.        m_pRoads[m_Route[nRouteIdx].m_nSRd] &&
  1712.        (m_Route[nRouteIdx].m_nSRd==m_nRdIdx ||
  1713.        m_Route[nRouteIdx].m_nSVIdx>=m_pRoads[m_Route[nRouteIdx].m_nSRd]->NumVerts()-1 &&
  1714.        m_Route[nRouteIdx].m_nSRd+1<=2 && m_pRoads[m_Route[nRouteIdx].m_nSRd+1]==0) &&
  1715.        m_nNumRoutes<10 &&
  1716.        nRouteIdx<=_nMaxRouteSplits)
  1717.     {
  1718.         nRouteIdx++;
  1719.         m_bFinished=true;
  1720.         EnumRoutes(nRouteIdx);
  1721.     }
  1722.     else if(m_Route[nRouteIdx].m_fCumDist<m_fRoutePlanDist && m_pRoads[2] &&
  1723.             !(m_Route[nRouteIdx].m_nSRd==2 && m_Route[nRouteIdx].m_nSVIdx==m_pRoads[2]->NumVerts()) &&
  1724.             m_nNumRoutes<10 &&
  1725.             nRouteIdx<=_nMaxRouteSplits)
  1726.     {
  1727.         nRouteIdx++;
  1728.         EnumRoutes(nRouteIdx);
  1729.     }
  1730.     else
  1731.     {
  1732.         // Save off all the routes to view as alternatives
  1733.         for(int i=0;i<=nRouteIdx;i++)
  1734.         {
  1735.             m_AltRoute[m_nNumRoutes][i]=m_Route[i];
  1736.             if(m_Route[i].m_nOnRoad==2)
  1737.                 m_naOffRoadRoute[m_nNumRoutes]=1;
  1738.             if(m_Route[i].m_nType==kBlockedTarget)
  1739.                 m_naBlockedRoute[m_nNumRoutes]=1;
  1740.         }
  1741.         m_naNumRouteVerts[m_nNumRoutes]=nRouteIdx+1;
  1742.         m_nNumRoutes++;
  1743.     }
  1744. }
  1745.  
  1746. void aiNavigation::CalcDestinationTarget(int nRouteIdx,Vector3& Position)
  1747. {
  1748.     Vector3 p, LTarget, RTarget;
  1749.     float fLX, fRX, fX, fZ, fLAngle, fRAngle, fLViewAngle, fRViewAngle;
  1750.     short nCRd, nLRd, nRRd, nCVIdx, nLVIdx, nRVIdx;
  1751.  
  1752.     nCRd=m_Route[nRouteIdx-1].m_nSRd;
  1753.     nCVIdx=m_Route[nRouteIdx-1].m_nSVIdx;
  1754.     int nNumVerts=m_pRoads[nCRd]->NumVerts();
  1755.  
  1756.     if(nRouteIdx>1 && m_Route[nRouteIdx-2].m_nType!=kTurnTarget)
  1757.     {
  1758.         nCVIdx++;
  1759.         if(nCVIdx==m_pRoads[nCRd]->NumVerts())
  1760.         {
  1761.             if(m_pRoads[nCRd+1])
  1762.             {
  1763.                 nCVIdx=0;
  1764.                 nCRd++;
  1765.                 nNumVerts=m_pRoads[nCRd]->NumVerts();
  1766.             }
  1767.             else
  1768.                 nCVIdx--;
  1769.         }
  1770.     }
  1771.  
  1772.     switch(m_nDestMapCompType)
  1773.     {
  1774.         case kRoad:
  1775.         {
  1776.  
  1777.             int nIndex=nNumVerts-nCVIdx;
  1778.             if(nIndex==nNumVerts)
  1779.                 nIndex--;
  1780.  
  1781.             Position=m_Route[nRouteIdx-1].Vtx;
  1782.  
  1783.             if(m_bDir[nCRd])
  1784.             {
  1785.                 // Init the left side
  1786.                 if(m_pRoads[nCRd]->Divider()==1)    // Do this just for jersey barriers.
  1787.                     LTarget=m_pRoads[nCRd]->CenterVertice(nCVIdx)-
  1788.                             (m_pRoads[nCRd]->VertXDir(nCVIdx))*(m_fLSideDist+1.f);
  1789.                 else 
  1790.                     LTarget=m_pRoads[nCRd]->LBoundary(nCVIdx)-
  1791.                             (m_pRoads[nCRd]->VertXDir(nCVIdx))*m_fLSideDist;
  1792.                 p=LTarget-m_Route[nRouteIdx-1].Vtx;
  1793.  
  1794.                 fLX=aiDot(p,(-(m_pRoads[nCRd]->VertXDir(nCVIdx))));
  1795.                 fZ=aiDot(p,(-(m_pRoads[nCRd]->VertZDir(nCVIdx))));
  1796.                 if(-0.01<fLX && fLX<0.01f)
  1797.                     fLX=-1.f;
  1798.                 fLViewAngle=atan2f(fLX,fZ);
  1799.  
  1800.                 // Now init the right side
  1801.                 RTarget=m_pRoads[nCRd]->RBoundary(nCVIdx)+
  1802.                         (m_pRoads[nCRd]->VertXDir(nCVIdx))*m_fRSideDist;
  1803.                 p=RTarget-m_Route[nRouteIdx-1].Vtx;
  1804.  
  1805.                 fRX=aiDot(p,(-(m_pRoads[nCRd]->VertXDir(nCVIdx))));
  1806.                 fZ=aiDot(p,(-(m_pRoads[nCRd]->VertZDir(nCVIdx))));
  1807.                 if(-0.01<fRX && fRX<0.01f)
  1808.                     fRX=1.f;
  1809.                 fRViewAngle=atan2f(fRX,fZ);
  1810.             }
  1811.             else
  1812.             {
  1813.                 Vector3 VDir=m_pRoads[nCRd]->CenterVertice(nNumVerts-nCVIdx-1)-
  1814.                              m_pRoads[nCRd]->RBoundary(nNumVerts-nCVIdx-1);
  1815.                 VDir.Normalize();
  1816.  
  1817.                 // Init the left side
  1818.                 LTarget=m_pRoads[nCRd]->RBoundary(nNumVerts-nCVIdx-1)+
  1819.                         VDir*(m_fLSideDist+1.f);
  1820.                 p=LTarget-m_Route[nRouteIdx-1].Vtx;
  1821.  
  1822.                 fLX=aiDot(p,m_pRoads[nCRd]->VertXDir(nIndex));
  1823.                 fZ=aiDot(p,m_pRoads[nCRd]->VertZDir(nIndex));
  1824.  
  1825.                 if(fLX>0.f && nRouteIdx==1)
  1826.                 {
  1827.                     Position=m_Route[nRouteIdx-1].Vtx+m_pRoads[nCRd]->VertXDir(nIndex)*fLX;
  1828.                     fLX=0.f;
  1829.                 }
  1830.                 if(fZ<1.f && nRouteIdx==1)fZ=1.f;
  1831.                 fLViewAngle=atan2f(fLX,fZ);
  1832.  
  1833.                 // Now init the right side
  1834.                 RTarget=m_pRoads[nCRd]->LBoundary(nNumVerts-nCVIdx-1)-
  1835.                         VDir*(m_fRSideDist+1.f);
  1836.                 p=RTarget-m_Route[nRouteIdx-1].Vtx;
  1837.  
  1838.                 fRX=aiDot(p,m_pRoads[nCRd]->VertXDir(nIndex));
  1839.                 fZ=aiDot(p,m_pRoads[nCRd]->VertZDir(nIndex));
  1840.  
  1841.                 if(fRX<0.f && nRouteIdx==1)
  1842.                 {
  1843.                     Position=m_Route[nRouteIdx-1].Vtx-m_pRoads[nCRd]->VertXDir(nIndex)*fRX;
  1844.                     fRX=0.f;
  1845.                 }
  1846.                 if(fZ<1.f && nRouteIdx==1)fZ=1.f;
  1847.                 fRViewAngle=atan2f(fRX,fZ);
  1848.             }
  1849.  
  1850.             // Check see if we can see the destination.
  1851.             int bKnownDir=false;
  1852.             nLVIdx=nRVIdx=nCVIdx;
  1853.             nLRd=nRRd=nCRd;
  1854.  
  1855.             nCVIdx++;
  1856.             if(nCVIdx==m_pRoads[nCRd]->NumVerts())
  1857.             {
  1858.                 if(m_pRoads[nCRd+1])
  1859.                 {
  1860.                     nCVIdx=0;
  1861.                     nCRd++;
  1862.                     nNumVerts=m_pRoads[nCRd]->NumVerts();
  1863.                 }
  1864.                 else
  1865.                     nCVIdx--;
  1866.             }
  1867.  
  1868.             for(nCVIdx;nCVIdx<m_pRoads[nCRd]->NumVerts();nCVIdx++)
  1869.             {
  1870.                 if(m_bDir[nCRd])
  1871.                 {
  1872.                     // First check the left side
  1873.                     if(m_pRoads[nCRd]->Divider()==1)    // Do this just for jersey barriers.
  1874.                         LTarget=m_pRoads[nCRd]->CenterVertice(nCVIdx)-
  1875.                                 (m_pRoads[nCRd]->VertXDir(nCVIdx))*(m_fLSideDist+1.f);
  1876.                     else 
  1877.                         LTarget=m_pRoads[nCRd]->LBoundary(nCVIdx)-
  1878.                                 (m_pRoads[nCRd]->VertXDir(nCVIdx))*(m_fLSideDist+1.f);
  1879.                     p=LTarget-Position;
  1880.  
  1881.                     if(!m_bDir[m_Route[nRouteIdx-1].m_nSRd])    // reverse
  1882.                     {
  1883.                         int nTmpIndex=m_pRoads[m_Route[nRouteIdx-1].m_nSRd]->NumVerts()-m_Route[nRouteIdx-1].m_nSVIdx-1;
  1884.                         if(nTmpIndex==m_pRoads[m_Route[nRouteIdx-1].m_nSRd]->NumVerts())
  1885.                             nTmpIndex--;
  1886.                         fX=aiDot(p,m_pRoads[m_Route[nRouteIdx-1].m_nSRd]->VertXDir(nTmpIndex));
  1887.                         fZ=aiDot(p,m_pRoads[m_Route[nRouteIdx-1].m_nSRd]->VertZDir(nTmpIndex));
  1888.                     }
  1889.                     else    // forward
  1890.                     {
  1891.                         fX=aiDot(p,(-(m_pRoads[m_Route[nRouteIdx-1].m_nSRd]->VertXDir(m_Route[nRouteIdx-1].m_nSVIdx))));
  1892.                         fZ=aiDot(p,(-(m_pRoads[m_Route[nRouteIdx-1].m_nSRd]->VertZDir(m_Route[nRouteIdx-1].m_nSVIdx))));
  1893.                     }
  1894.  
  1895.                     if(-0.01<fX && fX<0.01f)
  1896.                         fX=-1.f;
  1897.                     fLAngle=atan2f(fX,fZ);
  1898.  
  1899.                     if(fLAngle>fRViewAngle)        // Road must be going right
  1900.                     {
  1901.                         bKnownDir=true;
  1902.                         if(nRouteIdx>1 && nRRd==m_Route[nRouteIdx-1].m_nSRd && nRVIdx==m_Route[nRouteIdx-1].m_nSVIdx)
  1903.                         {
  1904.                             nRVIdx++;
  1905.                             if(nRVIdx>m_pRoads[nRRd]->NumVerts()-1)
  1906.                             {
  1907.                                 nRVIdx--;
  1908.                             }
  1909.                         }
  1910.                         nCRd=nRRd;
  1911.                         nCVIdx=nRVIdx;
  1912.  
  1913.                         if(m_pRoads[nCRd]->Divider()==1)    // Do this just for jersey barriers.
  1914.                             m_Route[nRouteIdx].Vtx=m_pRoads[nCRd]->CenterVertice(nCVIdx)+
  1915.                                                    (m_pRoads[nCRd]->VertXDir(nCVIdx))*(m_fRSideDist+1.f);
  1916.                         else 
  1917.                             m_Route[nRouteIdx].Vtx=m_pRoads[nCRd]->RBoundary(nCVIdx)+
  1918.                                                    (m_pRoads[nCRd]->VertXDir(nCVIdx))*(m_fRSideDist+1.f);
  1919.                         break;
  1920.                     }
  1921.                     else if(fLAngle>fLViewAngle-.001f)    // Tighten up the left side of the view frustrum
  1922.                     {
  1923.                         fLViewAngle=fLAngle;
  1924.                         nLVIdx=nCVIdx;
  1925.                         nLRd=nCRd;
  1926.                     }
  1927.  
  1928.                     // Now check the right side
  1929.                     RTarget=m_pRoads[nCRd]->RBoundary(nCVIdx)+
  1930.                             (m_pRoads[nCRd]->VertXDir(nCVIdx))*(m_fRSideDist+1.f);
  1931.                     p=RTarget-Position;
  1932.                     
  1933.                     if(!m_bDir[m_Route[nRouteIdx-1].m_nSRd])    // reverse
  1934.                     {
  1935.                         int nTmpIndex=m_pRoads[m_Route[nRouteIdx-1].m_nSRd]->NumVerts()-m_Route[nRouteIdx-1].m_nSVIdx-1;
  1936.                         if(nTmpIndex==m_pRoads[m_Route[nRouteIdx-1].m_nSRd]->NumVerts())
  1937.                             nTmpIndex--;
  1938.                         fX=aiDot(p,m_pRoads[m_Route[nRouteIdx-1].m_nSRd]->VertXDir(nTmpIndex));
  1939.                         fZ=aiDot(p,m_pRoads[m_Route[nRouteIdx-1].m_nSRd]->VertZDir(nTmpIndex));
  1940.                     }
  1941.                     else    // forward
  1942.                     {
  1943.                         fX=aiDot(p,(-(m_pRoads[m_Route[nRouteIdx-1].m_nSRd]->VertXDir(m_Route[nRouteIdx-1].m_nSVIdx))));
  1944.                         fZ=aiDot(p,(-(m_pRoads[m_Route[nRouteIdx-1].m_nSRd]->VertZDir(m_Route[nRouteIdx-1].m_nSVIdx))));
  1945.                     }
  1946.  
  1947.                     if(-0.01<fX && fX<0.01f)
  1948.                         fX=1.f;
  1949.                     fRAngle=atan2f(fX,fZ);
  1950.  
  1951.                     if(fRAngle<fLViewAngle)    // Road must be going left
  1952.                     {
  1953.                         bKnownDir=true;
  1954.                         if(nRouteIdx>1 && nLRd==m_Route[nRouteIdx-1].m_nSRd && nLVIdx==m_Route[nRouteIdx-1].m_nSVIdx)
  1955.                         {
  1956.                             nLVIdx++;
  1957.                             if(nLVIdx>m_pRoads[nLRd]->NumVerts()-1)
  1958.                             {
  1959.                                 nLVIdx--;
  1960.                             }
  1961.                         }
  1962.                         nCRd=nLRd;
  1963.                         nCVIdx=nLVIdx;
  1964.  
  1965.                         if(m_pRoads[nCRd]->Divider()==1)    // Do this just for jersey barriers.
  1966.                             m_Route[nRouteIdx].Vtx=m_pRoads[nCRd]->CenterVertice(nCVIdx)-
  1967.                                                    (m_pRoads[nCRd]->VertXDir(nCVIdx))*(m_fLSideDist+1.f);
  1968.                         else 
  1969.                             m_Route[nRouteIdx].Vtx=m_pRoads[nCRd]->LBoundary(nCVIdx)-
  1970.                                                    (m_pRoads[nCRd]->VertXDir(nCVIdx))*(m_fLSideDist+1.f);
  1971.                         break;
  1972.                     }
  1973.                     else if(fRAngle<fRViewAngle+.001f)    // Tighten up the right side of the view frustrum
  1974.                     {
  1975.                         fRViewAngle=fRAngle;
  1976.                         nRVIdx=nCVIdx;
  1977.                         nRRd=nCRd;
  1978.                     }
  1979.                 }
  1980.                 else
  1981.                 {
  1982.                     // First check the left side
  1983.                     Vector3 VDir=m_pRoads[nCRd]->CenterVertice(nNumVerts-nCVIdx-1)-
  1984.                                  m_pRoads[nCRd]->RBoundary(nNumVerts-nCVIdx-1);
  1985.                     VDir.Normalize();
  1986.  
  1987.                     LTarget=m_pRoads[nCRd]->RBoundary(nNumVerts-nCVIdx-1)+
  1988.                             VDir*(m_fLSideDist+1.f);
  1989.                     p=LTarget-Position;
  1990.  
  1991.                     if(!m_bDir[m_Route[nRouteIdx-1].m_nSRd])    // reverse
  1992.                     {
  1993.                         int nTmpIndex=m_pRoads[m_Route[nRouteIdx-1].m_nSRd]->NumVerts()-m_Route[nRouteIdx-1].m_nSVIdx-1;
  1994.                         if(nTmpIndex==m_pRoads[m_Route[nRouteIdx-1].m_nSRd]->NumVerts())
  1995.                             nTmpIndex--;
  1996.                         fX=aiDot(p,m_pRoads[m_Route[nRouteIdx-1].m_nSRd]->VertXDir(nTmpIndex));
  1997.                         fZ=aiDot(p,m_pRoads[m_Route[nRouteIdx-1].m_nSRd]->VertZDir(nTmpIndex));
  1998.                     }
  1999.                     else    // forward
  2000.                     {
  2001.                         fX=aiDot(p,(-(m_pRoads[m_Route[nRouteIdx-1].m_nSRd]->VertXDir(m_Route[nRouteIdx-1].m_nSVIdx))));
  2002.                         fZ=aiDot(p,(-(m_pRoads[m_Route[nRouteIdx-1].m_nSRd]->VertZDir(m_Route[nRouteIdx-1].m_nSVIdx))));
  2003.                     }
  2004.                     if(-0.01<fX && fX<0.01f)
  2005.                         fX=-1.f;
  2006.                     if(fZ<1.f)fZ=1.f;
  2007.                     fLAngle=atan2f(fX,fZ);
  2008.  
  2009.                     if(fLAngle>fRViewAngle)        // Road must be going right
  2010.                     {
  2011.                         bKnownDir=true;
  2012.                         nCRd=nRRd;
  2013.                         nCVIdx=nRVIdx;
  2014.                         nNumVerts=m_pRoads[nCRd]->NumVerts();
  2015.  
  2016.                         if(m_bDir[nCRd])
  2017.                         {
  2018.                             Vector3 Dir=m_pRoads[nCRd]->CenterVertice(nCVIdx)-
  2019.                                         m_pRoads[nCRd]->RBoundary(nCVIdx);
  2020.                             Dir.Normalize();
  2021.                             m_Route[nRouteIdx].Vtx=m_pRoads[nCRd]->RBoundary(nCVIdx)+
  2022.                                                    Dir*(m_fRSideDist+1.f);
  2023.                         }
  2024.                         else
  2025.                         {
  2026.                             Vector3 Dir=m_pRoads[nCRd]->CenterVertice(nNumVerts-nCVIdx-1)-
  2027.                                         m_pRoads[nCRd]->LBoundary(nNumVerts-nCVIdx-1);
  2028.                             Dir.Normalize();
  2029.                             m_Route[nRouteIdx].Vtx=m_pRoads[nCRd]->LBoundary(nNumVerts-nCVIdx-1)+
  2030.                                                     Dir*(m_fRSideDist+1.f);
  2031.                         }
  2032.                         break;
  2033.                     }
  2034.                     else if(fLAngle>fLViewAngle-.001f)    // Tighten up the left side of the view frustrum
  2035.                     {
  2036.                         fLViewAngle=fLAngle;
  2037.                         nLVIdx=nCVIdx;
  2038.                         nLRd=nCRd;
  2039.                     }
  2040.  
  2041.                     // Check the right side.
  2042.                     VDir=m_pRoads[nCRd]->CenterVertice(nNumVerts-nCVIdx-1)-
  2043.                          m_pRoads[nCRd]->RBoundary(nNumVerts-nCVIdx-1);
  2044.                     VDir.Normalize();
  2045.  
  2046.                     RTarget=m_pRoads[nCRd]->LBoundary(nNumVerts-nCVIdx-1)-
  2047.                             (m_pRoads[nCRd]->VertXDir(nIndex))*(m_fRSideDist+1.f);
  2048.                     p=RTarget-Position;
  2049.                     
  2050.                     if(!m_bDir[m_Route[nRouteIdx-1].m_nSRd])    // reverse
  2051.                     {
  2052.                         int nTmpIndex=m_pRoads[m_Route[nRouteIdx-1].m_nSRd]->NumVerts()-m_Route[nRouteIdx-1].m_nSVIdx-1;
  2053.                         if(nTmpIndex==m_pRoads[m_Route[nRouteIdx-1].m_nSRd]->NumVerts())
  2054.                             nTmpIndex--;
  2055.                         fX=aiDot(p,m_pRoads[m_Route[nRouteIdx-1].m_nSRd]->VertXDir(nTmpIndex));
  2056.                         fZ=aiDot(p,m_pRoads[m_Route[nRouteIdx-1].m_nSRd]->VertZDir(nTmpIndex));
  2057.                     }
  2058.                     else    // forward
  2059.                     {
  2060.                         fX=aiDot(p,(-(m_pRoads[m_Route[nRouteIdx-1].m_nSRd]->VertXDir(m_Route[nRouteIdx-1].m_nSVIdx))));
  2061.                         fZ=aiDot(p,(-(m_pRoads[m_Route[nRouteIdx-1].m_nSRd]->VertZDir(m_Route[nRouteIdx-1].m_nSVIdx))));
  2062.                     }
  2063.                     if(-0.01<fX && fX<0.01f)
  2064.                         fX=1.f;
  2065.                     if(fZ<1.f)fZ=1.f;
  2066.                     fRAngle=atan2f(fX,fZ);
  2067.  
  2068.                     if(fRAngle<fLViewAngle)    // Road must be going left
  2069.                     {
  2070.                         bKnownDir=true;
  2071.                         nCRd=nLRd;
  2072.                         nCVIdx=nLVIdx;
  2073.                         nNumVerts=m_pRoads[nCRd]->NumVerts();
  2074.  
  2075.                         if(m_bDir[nCRd])
  2076.                         {
  2077.                             Vector3 Dir=m_pRoads[nCRd]->CenterVertice(nCVIdx)-
  2078.                                         m_pRoads[nCRd]->LBoundary(nCVIdx);
  2079.                             Dir.Normalize();
  2080.                             if(m_pRoads[nCRd]->Divider()==1)    // Do this just for jersey barriers.
  2081.                                 m_Route[nRouteIdx].Vtx=m_pRoads[nCRd]->CenterVertice(nCVIdx)+
  2082.                                                        Dir*(m_fLSideDist+1.f);
  2083.                             else 
  2084.                             {
  2085.                                 m_Route[nRouteIdx].Vtx=m_pRoads[nCRd]->LBoundary(nCVIdx)+
  2086.                                                         Dir*(m_fLSideDist+1.f);
  2087.                             }
  2088.                         }
  2089.                         else
  2090.                         {
  2091.                             Vector3 Dir=m_pRoads[nCRd]->CenterVertice(nNumVerts-nCVIdx-1)-
  2092.                                         m_pRoads[nCRd]->RBoundary(nNumVerts-nCVIdx-1);
  2093.                             Dir.Normalize();
  2094.                             m_Route[nRouteIdx].Vtx=m_pRoads[nCRd]->RBoundary(nNumVerts-nCVIdx-1)+
  2095.                                                     Dir*(m_fLSideDist+1.f);
  2096.                         }
  2097.                         break;
  2098.                     }
  2099.                     else if(fRAngle<fRViewAngle+.001f)    // Tighten up the right side of the view frustrum
  2100.                     {
  2101.                         fRViewAngle=fRAngle;
  2102.                         nRVIdx=nCVIdx;
  2103.                         nRRd=nCRd;
  2104.                     }
  2105.                 }
  2106.             }
  2107.  
  2108.             if(nLVIdx<nRVIdx)
  2109.                 nCVIdx=nLVIdx;
  2110.             else
  2111.                 nCVIdx=nRVIdx;
  2112.  
  2113.             if(!bKnownDir)
  2114.             {
  2115.                 if(!m_bDir[nRRd] && 
  2116.                    (nRRd<nCRd || nRVIdx<m_pRoads[nRRd]->NumVerts()-1))            // road goes to the right
  2117.                 {
  2118.                     int nRd=m_Route[nRouteIdx-1].m_nSRd;
  2119.                     int nVert=m_Route[nRouteIdx-1].m_nSVIdx;
  2120.                     if(nVert<1)
  2121.                         nVert=1;
  2122.  
  2123.                     if(m_bDir[nRd])
  2124.                     {
  2125.                         p=m_Route[nRouteIdx-1].Vtx-m_pRoads[nRd]->RBoundary(nVert);
  2126.                         float fMySide=aiDot(p,m_pRoads[nRd]->VertXDir(nVert));
  2127.                         m_Route[nRouteIdx].Vtx=m_pRoads[nRRd]->LBoundary(m_pRoads[nRRd]->NumVerts()-nRVIdx-1)+
  2128.                                                m_pRoads[nRd]->VertXDir(nVert)*fMySide;
  2129.                     }
  2130.                     else
  2131.                     {
  2132.                         nNumVerts=m_pRoads[nRd]->NumVerts();
  2133.                         p=m_Route[nRouteIdx-1].Vtx-m_pRoads[nRd]->LBoundary(nNumVerts-nVert-1);
  2134.                         float fMySide=aiDot(p,-m_pRoads[nRd]->VertXDir(nNumVerts-nVert));
  2135.                         m_Route[nRouteIdx].Vtx=m_pRoads[nRRd]->LBoundary(m_pRoads[nRRd]->NumVerts()-nRVIdx-1)-
  2136.                                                m_pRoads[nRd]->VertXDir(nNumVerts-nVert)*fMySide;
  2137.                     }
  2138.                     nCRd=nRRd;
  2139.                     nCVIdx=nRVIdx;
  2140.                     m_bFinished=false;
  2141.                 }
  2142.                 else if(!m_bDir[nLRd] && 
  2143.                         (nLRd<nCRd || nLVIdx<m_pRoads[nLRd]->NumVerts()-1))    // road goes to the left
  2144.                 {
  2145.                     int nRd=m_Route[nRouteIdx-1].m_nSRd;
  2146.                     int nVert=m_Route[nRouteIdx-1].m_nSVIdx;
  2147.                     if(nVert<1)
  2148.                         nVert=1;
  2149.                     if(m_bDir[nRd])
  2150.                     {
  2151.                         p=m_Route[nRouteIdx-1].Vtx-m_pRoads[nRd]->RBoundary(nVert);
  2152.                         float fMySide=aiDot(p,m_pRoads[nRd]->VertXDir(nVert));
  2153.                         m_Route[nRouteIdx].Vtx=m_pRoads[nLRd]->LBoundary(m_pRoads[nLRd]->NumVerts()-nLVIdx-1)+
  2154.                                                m_pRoads[nRd]->VertXDir(nVert)*fMySide;
  2155.                     }
  2156.                     else
  2157.                     {
  2158.                         nNumVerts=m_pRoads[nRd]->NumVerts();
  2159.                         p=m_Route[nRouteIdx-1].Vtx-m_pRoads[nRd]->LBoundary(nNumVerts-nVert-1);
  2160.                         float fMySide=aiDot(p,-m_pRoads[nRd]->VertXDir(nNumVerts-nVert));
  2161.                         m_Route[nRouteIdx].Vtx=m_pRoads[nLRd]->LBoundary(m_pRoads[nLRd]->NumVerts()-nLVIdx-1)-
  2162.                                                m_pRoads[nRd]->VertXDir(nNumVerts-nVert)*fMySide;
  2163.                     }
  2164.                     nCRd=nLRd;
  2165.                     nCVIdx=nLVIdx;
  2166.                     m_bFinished=false;
  2167.                 }
  2168.  
  2169.                 else if(m_bDir[nRRd] && 
  2170.                    (nRRd<nCRd || nRVIdx<m_pRoads[nRRd]->NumVerts()-1))            // road goes to the right
  2171.                 {
  2172.                     float fMySide;
  2173.                     int nRd=m_Route[nRouteIdx-1].m_nSRd;
  2174.                     int nVert=m_Route[nRouteIdx-1].m_nSVIdx;
  2175.                     if(m_bDir[nRd])
  2176.                     {
  2177.                         p=m_Route[nRouteIdx-1].Vtx-m_pRoads[nRd]->RBoundary(nVert);
  2178.                         fMySide=aiDot(p,m_pRoads[nRd]->VertXDir(nVert));
  2179.                     }
  2180.                     else
  2181.                     {
  2182.                         p=m_Route[nRouteIdx-1].Vtx-m_pRoads[nRd]->LBoundary(m_pRoads[nRd]->NumVerts()-nVert-1);
  2183.                         fMySide=aiDot(p,-m_pRoads[nRd]->VertXDir(m_pRoads[nRd]->NumVerts()-nVert-1));
  2184.                     }
  2185.                     if(fMySide<m_fRSideDist)
  2186.                         fMySide=m_fRSideDist;
  2187.  
  2188.                     m_Route[nRouteIdx].Vtx=m_pRoads[nRRd]->RBoundary(nRVIdx)+
  2189.                                            m_pRoads[nRRd]->VertXDir(nRVIdx)*fMySide;
  2190.  
  2191.                     nCRd=nRRd;
  2192.                     nCVIdx=nRVIdx;
  2193.                     m_bFinished=false;
  2194.                 }
  2195.                 else if(m_bDir[nLRd] && 
  2196.                         (nLRd<nCRd || nLVIdx<m_pRoads[nLRd]->NumVerts()-1))    // road goes to the left
  2197.                 {
  2198.                     float fMySide;
  2199.                     int nRd=m_Route[nRouteIdx-1].m_nSRd;
  2200.                     int nVert=m_Route[nRouteIdx-1].m_nSVIdx;
  2201.                     if(m_bDir[nRd])
  2202.                     {
  2203.                         p=m_Route[nRouteIdx-1].Vtx-m_pRoads[nRd]->LBoundary(nVert);
  2204.                         fMySide=aiDot(p,-m_pRoads[nRd]->VertXDir(nVert));
  2205.                     }
  2206.                     else
  2207.                     {
  2208.                         p=m_Route[nRouteIdx-1].Vtx-m_pRoads[nRd]->RBoundary(m_pRoads[nRd]->NumVerts()-nVert-1);
  2209.                         fMySide=aiDot(p,m_pRoads[nRd]->VertXDir(m_pRoads[nRd]->NumVerts()-nVert-1));
  2210.                     }
  2211.                     if(fMySide<m_fLSideDist)
  2212.                         fMySide=m_fLSideDist;
  2213.  
  2214.                     m_Route[nRouteIdx].Vtx=m_pRoads[nLRd]->LBoundary(nLVIdx)-
  2215.                                            m_pRoads[nLRd]->VertXDir(nLVIdx)*fMySide;
  2216.                     nCRd=nLRd;
  2217.                     nCVIdx=nLVIdx;
  2218.                     m_bFinished=false;
  2219.                 }
  2220.             }
  2221.  
  2222.             int nDestIdx=m_pRoads[nCRd]->RoadVertice(m_Destination,m_bDir[nCRd],-1);
  2223.             if(nCVIdx>=nDestIdx)
  2224.             {
  2225.                 SetTargetPtToDestination(nRouteIdx);
  2226.                 return;
  2227.             }
  2228.  
  2229.             m_Route[nRouteIdx].m_nType=kRoadTarget;
  2230.             m_Route[nRouteIdx].m_nSVIdx=nCVIdx;
  2231.             m_Route[nRouteIdx].m_nSRd=nCRd;
  2232.             m_Route[nRouteIdx].m_nTurnId=nCRd;
  2233.             m_Route[nRouteIdx].Vtx.y+=1.f;
  2234.  
  2235.             m_Route[nRouteIdx].m_fCumDist=m_Route[nRouteIdx-1].m_fCumDist+
  2236.                                           aiDist(m_Route[nRouteIdx-1].Vtx,m_Route[nRouteIdx].Vtx);
  2237.             if(nRouteIdx==1)
  2238.             {
  2239.                 Vector3 TargetDir=m_Route[nRouteIdx].Vtx-m_Car.GetMatrix().Pos;
  2240.                 m_Route[nRouteIdx].m_fCumAngle=(-m_Car.GetMatrix().ZDir).Angle(TargetDir);
  2241.             }
  2242.             else
  2243.             {
  2244.                 Vector3 TargetDir=m_Route[nRouteIdx].Vtx-m_Route[nRouteIdx-1].Vtx;
  2245.                 Vector3 PTargetDir=m_Route[nRouteIdx-1].Vtx-m_Route[nRouteIdx-2].Vtx;
  2246.                 m_Route[nRouteIdx].m_fCumAngle=m_Route[nRouteIdx-1].m_fCumAngle+
  2247.                                                PTargetDir.Angle(TargetDir);
  2248.             }
  2249.             break;
  2250.         }
  2251.         case kIntersection:
  2252.         {
  2253.             SetTargetPtToDestination(nRouteIdx);
  2254.             return;
  2255.         }
  2256.     }
  2257. }
  2258.  
  2259. void aiNavigation::SetTargetPtToDestination(int nRouteIdx)
  2260. {
  2261.     if(m_fDestOffset==0.f)
  2262.         m_Route[nRouteIdx].Vtx=m_Destination;
  2263.     else
  2264.     {
  2265.         float fDist=aiDist(m_Route[nRouteIdx-1].Vtx,m_Destination);
  2266.         if(fDist<m_fDestOffset)
  2267.             m_Route[nRouteIdx].Vtx=m_Route[nRouteIdx-1].Vtx;
  2268.         else
  2269.             m_Route[nRouteIdx].Vtx.Lerp((fDist-m_fDestOffset)/fDist,
  2270.                                             m_Route[nRouteIdx-1].Vtx,m_Destination);
  2271.     }
  2272.     m_Route[nRouteIdx].Vtx.y+=1.f;
  2273.     m_Route[nRouteIdx].m_nType=m_Route[nRouteIdx-1].m_nType;
  2274.     m_Route[nRouteIdx].m_nSVIdx=m_Route[nRouteIdx-1].m_nSVIdx;
  2275.     m_Route[nRouteIdx].m_nSRd=m_Route[nRouteIdx-1].m_nSRd;
  2276.     m_Route[nRouteIdx].m_nTurnId=m_Route[nRouteIdx-1].m_nTurnId;
  2277.  
  2278.     m_Route[nRouteIdx].m_fCumDist=9999.f;
  2279.     if(nRouteIdx==1)
  2280.     {
  2281.         Vector3 TargetDir=m_Route[nRouteIdx].Vtx-m_Car.GetMatrix().Pos;
  2282.         m_Route[nRouteIdx].m_fCumAngle=(-m_Car.GetMatrix().ZDir).Angle(TargetDir);
  2283.     }
  2284.     else
  2285.     {
  2286.         Vector3 TargetDir=m_Route[nRouteIdx].Vtx-m_Route[nRouteIdx-1].Vtx;
  2287.         Vector3 PTargetDir=m_Route[nRouteIdx-1].Vtx-m_Route[nRouteIdx-2].Vtx;
  2288.         m_Route[nRouteIdx].m_fCumAngle=m_Route[nRouteIdx-1].m_fCumAngle+
  2289.                                        PTargetDir.Angle(TargetDir);
  2290.     }
  2291. }
  2292.  
  2293. void aiNavigation::CalcRoadTarget(int nRouteIdx,Vector3& Position)
  2294. {
  2295.     Vector3 p, LTarget, RTarget;
  2296.     float fLX, fRX, fX, fZ, fLAngle, fRAngle;
  2297.     float fLViewAngle, fRViewAngle, fSideRatio, fSideDist;
  2298.     short nCRd, nLRd, nRRd, nCVIdx, nLVIdx, nRVIdx;
  2299.     int bKnownDir=false;
  2300.  
  2301.     nCRd=m_Route[nRouteIdx-1].m_nSRd;
  2302.     nCVIdx=m_Route[nRouteIdx-1].m_nSVIdx;
  2303.     int nNumVerts=m_pRoads[nCRd]->NumVerts();
  2304.  
  2305.     if(nRouteIdx>1 && m_Route[nRouteIdx-2].m_nType!=kTurnTarget)
  2306.     {
  2307.         nCVIdx++;
  2308.         if(nCVIdx>=m_pRoads[nCRd]->NumVerts())
  2309.         {
  2310.             if(m_pRoads[nCRd+1])
  2311.             {
  2312.                 nCVIdx=0;
  2313.                 nCRd++;
  2314.                 nNumVerts=m_pRoads[nCRd]->NumVerts();
  2315.             }
  2316.             else
  2317.                 nCVIdx--;
  2318.         }
  2319.     }
  2320.  
  2321.     int nIndex=nNumVerts-nCVIdx;
  2322.     if(nIndex==nNumVerts)
  2323.         nIndex--;
  2324.  
  2325.     Position=m_Route[nRouteIdx-1].Vtx;
  2326.  
  2327.     // Initialize the view angles.
  2328.     Vector3 VDir=m_pRoads[nCRd]->CenterVertice(nCVIdx)-
  2329.                  m_pRoads[nCRd]->RBoundary(nCVIdx);
  2330.     VDir.Normalize();
  2331.  
  2332.     // Determine which side of the road we are on.
  2333.     p=m_Route[nRouteIdx-1].Vtx-m_pRoads[nCRd]->CenterVertice(nCVIdx);
  2334.     fSideDist=aiDot(p,-m_pRoads[nCRd]->VertXDir(nCVIdx));
  2335.  
  2336.     // Init the left side
  2337.     if(m_pRoads[nCRd]->Divider() && fSideDist>=0.f)    // Do this just for jersey barriers.
  2338.         LTarget=m_pRoads[nCRd]->CenterVertice(nCVIdx)-VDir*(m_fLSideDist+1.5f);
  2339.     else 
  2340.         LTarget=m_pRoads[nCRd]->LBoundary(nCVIdx)-VDir*(m_fLSideDist+1.f);
  2341.     p=LTarget-m_Route[nRouteIdx-1].Vtx;
  2342.  
  2343.     fLX=aiDot(p,(-(m_pRoads[nCRd]->VertXDir(nCVIdx))));
  2344.     fZ=aiDot(p,(-(m_pRoads[nCRd]->VertZDir(nCVIdx))));
  2345.  
  2346.     if(fZ<1.f && nRouteIdx==1)fZ=1.f;
  2347.     fLViewAngle=atan2f(fLX,fZ);
  2348.  
  2349.     // Now init the right side
  2350.     if(m_pRoads[nCRd]->Divider() && fSideDist<0.f)    // Do this just for jersey barriers.
  2351.         RTarget=m_pRoads[nCRd]->CenterVertice(nCVIdx)+VDir*(m_fRSideDist+1.5f);
  2352.     else 
  2353.         RTarget=m_pRoads[nCRd]->RBoundary(nCVIdx)+VDir*(m_fRSideDist+1.f);
  2354.     p=RTarget-m_Route[nRouteIdx-1].Vtx;
  2355.  
  2356.     fRX=aiDot(p,(-(m_pRoads[nCRd]->VertXDir(nCVIdx))));
  2357.     fZ=aiDot(p,(-(m_pRoads[nCRd]->VertZDir(nCVIdx))));
  2358.  
  2359.     if(fZ<1.f && nRouteIdx==1)fZ=1.f;
  2360.     fRViewAngle=atan2f(fRX,fZ);
  2361.  
  2362.  
  2363.  
  2364.     // Set the default target
  2365.     float fMaxSDist;
  2366.     fMaxSDist=m_pRoads[nCRd]->LaneWidth(m_pRoads[nCRd]->NumLanes(1)-1,1,1)-
  2367.               m_fRSideDist-1.f;
  2368.     if(nRouteIdx==1)
  2369.     {
  2370.         if(nCVIdx>0)
  2371.         {    
  2372.             p=m_pRoads[nCRd]->CenterVertice(nCVIdx)-Position;
  2373.             m_fSideDist=aiDot(p,m_pRoads[nCRd]->VertXDir(nCVIdx));
  2374.             m_fSideDist=aiClamp(m_fSideDist,-fMaxSDist,fMaxSDist);
  2375.         }
  2376.         fSideDist=m_fSideDist;
  2377.     }
  2378.     else
  2379.     {
  2380.         p=m_pRoads[nCRd]->CenterVertice(nCVIdx)-Position;
  2381.         fSideDist=aiDot(p,m_pRoads[nCRd]->VertXDir(nCVIdx));
  2382.         fSideDist=aiClamp(fSideDist,-fMaxSDist,fMaxSDist);
  2383.     }
  2384.     fSideRatio=fSideDist/fMaxSDist;
  2385.     m_Route[nRouteIdx].Vtx.Lerp(fSideRatio,m_pRoads[nCRd]->CenterVertice(nNumVerts-1),
  2386.                                 m_pRoads[nCRd]->RBoundary(nNumVerts-1)+
  2387.                                 m_pRoads[nCRd]->VertXDir(nCVIdx)*(m_fRSideDist+1.f));
  2388.  
  2389.  
  2390.     // Check see if we can see the end of the road.
  2391.     for(nCVIdx;nCVIdx<m_pRoads[nCRd]->NumVerts();nCVIdx++)
  2392.     {
  2393.         // First check the left side
  2394.         Vector3 VDir=m_pRoads[nCRd]->CenterVertice(nCVIdx)-
  2395.                      m_pRoads[nCRd]->RBoundary(nCVIdx+nNumVerts);
  2396.         VDir.Normalize();
  2397.  
  2398.         if(m_pRoads[nCRd]->Divider() && fSideDist>=0.f)    // Do this just for jersey barriers.
  2399.             LTarget=m_pRoads[nCRd]->CenterVertice(nCVIdx)-VDir*(m_fLSideDist+1.5f);
  2400.         else 
  2401.             LTarget=m_pRoads[nCRd]->LBoundary(nCVIdx)-VDir*(m_fLSideDist+1.f);
  2402.         p=LTarget-Position;
  2403.  
  2404.         if(m_bDir[m_Route[nRouteIdx-1].m_nSRd])    // forward
  2405.         {
  2406.             fX=aiDot(p,(-(m_pRoads[m_Route[nRouteIdx-1].m_nSRd]->VertXDir(m_Route[nRouteIdx-1].m_nSVIdx))));
  2407.             fZ=aiDot(p,(-(m_pRoads[m_Route[nRouteIdx-1].m_nSRd]->VertZDir(m_Route[nRouteIdx-1].m_nSVIdx))));
  2408.         }
  2409.         else    // reverse
  2410.         {
  2411.             int nTmpIndex=m_pRoads[m_Route[nRouteIdx-1].m_nSRd]->NumVerts()-m_Route[nRouteIdx-1].m_nSVIdx;
  2412.             if(nTmpIndex==m_pRoads[m_Route[nRouteIdx-1].m_nSRd]->NumVerts())
  2413.                 nTmpIndex--;
  2414.             fX=aiDot(p,m_pRoads[m_Route[nRouteIdx-1].m_nSRd]->VertXDir(nTmpIndex));
  2415.             fZ=aiDot(p,m_pRoads[m_Route[nRouteIdx-1].m_nSRd]->VertZDir(nTmpIndex));
  2416.         }
  2417.         if(-0.01<fX && fX<0.01f)
  2418.             fX=-1.f;
  2419.         if(fZ<1.f)fZ=1.f;
  2420.         fLAngle=atan2f(fX,fZ);
  2421.  
  2422.         if(fLAngle>fRViewAngle)        // Road must be going right
  2423.         {
  2424.             bKnownDir=true;
  2425.             nCRd=nRRd;
  2426.             nCVIdx=nRVIdx;
  2427.             nNumVerts=m_pRoads[nCRd]->NumVerts();
  2428.  
  2429.             if(m_bDir[nCRd])
  2430.             {
  2431.                 Vector3 Dir=m_pRoads[nCRd]->CenterVertice(nCVIdx)-
  2432.                             m_pRoads[nCRd]->RBoundary(nCVIdx);
  2433.                 Dir.Normalize();
  2434.                 if(m_pRoads[nCRd]->Divider() && fSideDist<0.f)    // Do this just for jersey barriers.
  2435.                     m_Route[nRouteIdx].Vtx=m_pRoads[nCRd]->CenterVertice(nCVIdx)+Dir*(m_fRSideDist+1.5f);
  2436.                 else 
  2437.                     m_Route[nRouteIdx].Vtx=m_pRoads[nCRd]->RBoundary(nCVIdx)+
  2438.                                            Dir*(m_fRSideDist+1.f);
  2439.                 
  2440.                 // if road is sharp turn right
  2441.                 if(nCVIdx==m_pRoads[nCRd]->NumVerts()-1 && m_fRdAngle[nCRd]>_fTurnThreshold)
  2442.                     m_Route[nRouteIdx].Vtx=m_IntersectionPt[nCRd];
  2443.             }
  2444.             else
  2445.             {
  2446.                 Vector3 Dir=m_pRoads[nCRd]->CenterVertice(nNumVerts-nCVIdx-1)-
  2447.                             m_pRoads[nCRd]->LBoundary(nNumVerts-nCVIdx-1);
  2448.                 Dir.Normalize();
  2449.                 if(m_pRoads[nCRd]->Divider() && fSideDist<0.f)    // Do this just for jersey barriers.
  2450.                     m_Route[nRouteIdx].Vtx=m_pRoads[nCRd]->CenterVertice(nNumVerts-nCVIdx-1)+
  2451.                                             Dir*(m_fRSideDist+1.5f);
  2452.                 else
  2453.                     m_Route[nRouteIdx].Vtx=m_pRoads[nCRd]->LBoundary(nNumVerts-nCVIdx-1)+
  2454.                                             Dir*(m_fRSideDist+1.f);
  2455.             }
  2456.             break;
  2457.         }
  2458.         else if(fLAngle>fLViewAngle-.001f)    // Tighten up the left side of the view frustrum
  2459.         {
  2460.             fLViewAngle=fLAngle;
  2461.             nLVIdx=nCVIdx;
  2462.             nLRd=nCRd;
  2463.         }
  2464.  
  2465.         // Now check the right side
  2466.         VDir=m_pRoads[nCRd]->CenterVertice(nCVIdx)-
  2467.              m_pRoads[nCRd]->RBoundary(nCVIdx);
  2468.         VDir.Normalize();
  2469.  
  2470.         if(m_pRoads[nCRd]->Divider() && fSideDist<0.f)    // Do this just for jersey barriers.
  2471.             RTarget=m_pRoads[nCRd]->CenterVertice(nCVIdx)+VDir*(m_fRSideDist+1.5f);
  2472.         else 
  2473.             RTarget=m_pRoads[nCRd]->RBoundary(nCVIdx)+VDir*(m_fRSideDist+1.f);
  2474.         p=RTarget-Position;
  2475.         
  2476.         if(m_bDir[m_Route[nRouteIdx-1].m_nSRd])    // forward
  2477.         {
  2478.             fX=aiDot(p,(-(m_pRoads[m_Route[nRouteIdx-1].m_nSRd]->VertXDir(m_Route[nRouteIdx-1].m_nSVIdx))));
  2479.             fZ=aiDot(p,(-(m_pRoads[m_Route[nRouteIdx-1].m_nSRd]->VertZDir(m_Route[nRouteIdx-1].m_nSVIdx))));
  2480.         }
  2481.         else    // reverse
  2482.         {
  2483.             int nTmpIndex=m_pRoads[m_Route[nRouteIdx-1].m_nSRd]->NumVerts()-m_Route[nRouteIdx-1].m_nSVIdx;
  2484.             if(nTmpIndex==m_pRoads[m_Route[nRouteIdx-1].m_nSRd]->NumVerts())
  2485.                 nTmpIndex--;
  2486.             fX=aiDot(p,m_pRoads[m_Route[nRouteIdx-1].m_nSRd]->VertXDir(nTmpIndex));
  2487.             fZ=aiDot(p,m_pRoads[m_Route[nRouteIdx-1].m_nSRd]->VertZDir(nTmpIndex));
  2488.         }
  2489.  
  2490.         if(-0.01<fX && fX<0.01f)
  2491.             fX=0.f;
  2492.         if(fZ<1.f)fZ=1.f;
  2493.         fRAngle=atan2f(fX,fZ);
  2494.  
  2495.         if(fRAngle<fLViewAngle)    // Road must be going left
  2496.         {
  2497.             bKnownDir=true;
  2498.             nCRd=nLRd;
  2499.             nCVIdx=nLVIdx;
  2500.             nNumVerts=m_pRoads[nCRd]->NumVerts();
  2501.  
  2502.             if(m_bDir[nCRd])
  2503.             {
  2504.                 Vector3 Dir=m_pRoads[nCRd]->CenterVertice(nCVIdx)-
  2505.                             m_pRoads[nCRd]->LBoundary(nCVIdx);
  2506.                 Dir.Normalize();
  2507.                 if(m_pRoads[nCRd]->Divider() && fSideDist>=0.f)    // Do this just for jersey barriers.
  2508.                     m_Route[nRouteIdx].Vtx=m_pRoads[nCRd]->CenterVertice(nCVIdx)+
  2509.                                             Dir*(m_fLSideDist+1.5f);
  2510.                 else 
  2511.                     m_Route[nRouteIdx].Vtx=m_pRoads[nCRd]->LBoundary(nCVIdx)+
  2512.                                             Dir*(m_fLSideDist+1.f);
  2513.  
  2514.                 // if road is sharp turn left or right
  2515.                 if(nCVIdx==m_pRoads[nCRd]->NumVerts()-1 && m_fRdAngle[nCRd]<-_fTurnThreshold)
  2516.                     m_Route[nRouteIdx].Vtx=m_IntersectionPt[nCRd];
  2517.             }
  2518.             else
  2519.             {
  2520.                 Vector3 Dir=m_pRoads[nCRd]->CenterVertice(nNumVerts-nCVIdx-1)-
  2521.                             m_pRoads[nCRd]->RBoundary(nNumVerts-nCVIdx-1);
  2522.                 Dir.Normalize();
  2523.                 if(m_pRoads[nCRd]->Divider() && fSideDist>=0.f)    // Do this just for jersey barriers.
  2524.                     m_Route[nRouteIdx].Vtx=m_pRoads[nCRd]->CenterVertice(nNumVerts-nCVIdx-1)+
  2525.                                             Dir*(m_fLSideDist+1.5f);
  2526.                 else
  2527.                     m_Route[nRouteIdx].Vtx=m_pRoads[nCRd]->RBoundary(nNumVerts-nCVIdx-1)+
  2528.                                             Dir*(m_fLSideDist+1.f);
  2529.             }
  2530.             break;
  2531.         }
  2532.         else if(fRAngle<fRViewAngle+.001f)    // Tighten up the right side of the view frustrum
  2533.         {
  2534.             fRViewAngle=fRAngle;
  2535.             nRVIdx=nCVIdx;
  2536.             nRRd=nCRd;
  2537.         }
  2538.  
  2539.         // Check to see if we need to keep looking at the upcomming roads.
  2540.         if(nCVIdx==m_pRoads[nCRd]->NumVerts()-1)
  2541.         {
  2542.             // if road is sharp turn left or right
  2543.             if(nCRd<2)
  2544.             {
  2545.                 if(m_fRdAngle[nCRd]<-_fTurnThreshold && nCVIdx==nLVIdx && nLRd==nCRd && 
  2546.                     nCVIdx==nRVIdx && nRRd==nCRd)
  2547.                 {
  2548.                     if(nLVIdx==m_pRoads[nLRd]->NumVerts()-1)
  2549.                     {
  2550.                         // then set to the intersection point
  2551.                         bKnownDir=true;
  2552.                         m_Route[nRouteIdx].Vtx=m_IntersectionPt[nCRd];
  2553.                     }
  2554.                     break;
  2555.                 }
  2556.                 else if(_fTurnThreshold<m_fRdAngle[nCRd] && nCVIdx==nLVIdx && nLRd==nCRd && 
  2557.                         nCVIdx==nRVIdx && nRRd==nCRd)
  2558.                 {
  2559.                     if(nRVIdx==m_pRoads[nRRd]->NumVerts()-1)
  2560.                     {
  2561.                         // then set to the intersection point
  2562.                         bKnownDir=true;
  2563.                         m_Route[nRouteIdx].Vtx=m_IntersectionPt[nCRd];
  2564.                     }
  2565.                     break;
  2566.                 }
  2567.             }
  2568.  
  2569.             float fDist=aiDist(Position,m_Route[nRouteIdx].Vtx);
  2570.             if(m_Route[nRouteIdx-1].m_fCumDist+fDist<m_fRoutePlanDist && nCRd<2)
  2571.             {
  2572.                 if(nCRd+1>=m_nRdIdx)
  2573.                 {
  2574.                     if(m_nDestMapCompType==kIntersection /*&& m_nDestMapCompId==m_nCurMapCompIdx*/)
  2575.                     {
  2576.                         SetTargetPtToDestination(nRouteIdx);
  2577.                         return;
  2578.                     }
  2579.                     else
  2580.                         m_bFinished=true;
  2581.                     break;
  2582.                 }
  2583.     
  2584.                 if(m_pRoads[nCRd+1])
  2585.                 {
  2586.                     nCRd++;
  2587.                     nCVIdx=-1;
  2588.                     nNumVerts=m_pRoads[nCRd]->NumVerts();
  2589.  
  2590.                     float fMaxSDist=m_pRoads[nCRd]->LaneWidth(m_pRoads[nCRd]->NumLanes(1)-1,1,1)-
  2591.                                     m_fRSideDist-1.f;
  2592.                     if(!m_bDir[nCRd])    // reverse
  2593.                     {
  2594.                         p=m_pRoads[nCRd]->CenterVertice(nNumVerts-1)-m_Route[nRouteIdx].Vtx;
  2595.                         float fMySDist=aiDot(p,-m_pRoads[nCRd]->VertXDir(nNumVerts-1));
  2596.                         fMySDist=aiClamp(fMySDist,-fMaxSDist,fMaxSDist);
  2597.                         float fSideRatio=fMySDist/fMaxSDist;
  2598.  
  2599.                         m_Route[nRouteIdx].Vtx.Lerp(fSideRatio,m_pRoads[nCRd]->CenterVertice(0),
  2600.                                                     m_pRoads[nCRd]->LBoundary(0)-
  2601.                                                     m_pRoads[nCRd]->VertXDir(0)*(m_fRSideDist+1.5f));
  2602.                     }
  2603.                     else
  2604.                     {
  2605.                         p=m_pRoads[nCRd]->CenterVertice(0)-m_Route[nRouteIdx].Vtx;
  2606.                         float fMySDist=aiDot(p,m_pRoads[nCRd]->VertXDir(0));
  2607.                         fMySDist=aiClamp(fMySDist,-fMaxSDist,fMaxSDist);
  2608.                         float fSideRatio=fMySDist/fMaxSDist;
  2609.  
  2610.                         m_Route[nRouteIdx].Vtx.Lerp(fSideRatio,m_pRoads[nCRd]->CenterVertice(nNumVerts-1),
  2611.                                                     m_pRoads[nCRd]->RBoundary(nNumVerts-1)+
  2612.                                                     m_pRoads[nCRd]->VertXDir(nNumVerts-1)*(m_fRSideDist+1.5f));
  2613.                     }
  2614.                 }
  2615.                 else
  2616.                     break;
  2617.             }
  2618.             else
  2619.                 break;
  2620.         }
  2621.     }
  2622.  
  2623.     if(!bKnownDir)
  2624.     {
  2625.         if(!m_bDir[nRRd] && 
  2626.            (nRRd<nCRd || nRVIdx<m_pRoads[nRRd]->NumVerts()-1))            // road goes to the right
  2627.         {
  2628.             if(m_pRoads[nRRd]->Divider()==1 && fSideDist<0.f)    // Do this just for jersey barriers.
  2629.             {
  2630.                 nNumVerts=m_pRoads[nRRd]->NumVerts();
  2631.                 Vector3 Dir=m_pRoads[nRRd]->CenterVertice(nNumVerts-nRVIdx-1)-
  2632.                             m_pRoads[nRRd]->RBoundary(nNumVerts-nRVIdx-1);
  2633.                 Dir.Normalize();
  2634.                 m_Route[nRouteIdx].Vtx=m_pRoads[nRRd]->CenterVertice(nNumVerts-nRVIdx-1)-Dir*(m_fRSideDist+1.5f);
  2635.             }
  2636.             else
  2637.             {
  2638.                 int nRd=m_Route[nRouteIdx-1].m_nSRd;
  2639.                 int nVert=m_Route[nRouteIdx-1].m_nSVIdx;
  2640.                 if(nVert<1)
  2641.                     nVert=1;
  2642.  
  2643.                 if(m_bDir[nRd])
  2644.                 {
  2645.                     p=m_Route[nRouteIdx-1].Vtx-m_pRoads[nRd]->RBoundary(nVert);
  2646.                     float fMySide=aiDot(p,m_pRoads[nRd]->VertXDir(nVert));
  2647.                     m_Route[nRouteIdx].Vtx=m_pRoads[nRRd]->LBoundary(m_pRoads[nRRd]->NumVerts()-nRVIdx-1)+
  2648.                                            m_pRoads[nRd]->VertXDir(nVert)*fMySide;
  2649.                 }
  2650.                 else
  2651.                 {
  2652.                     nNumVerts=m_pRoads[nRd]->NumVerts();
  2653.                     p=m_Route[nRouteIdx-1].Vtx-m_pRoads[nRd]->LBoundary(nNumVerts-nVert-1);
  2654.                     float fMySide=aiDot(p,-m_pRoads[nRd]->VertXDir(nNumVerts-nVert));
  2655.                     if(fMySide<0.f)
  2656.                         fMySide=0.f;
  2657.                     m_Route[nRouteIdx].Vtx=m_pRoads[nRRd]->LBoundary(m_pRoads[nRRd]->NumVerts()-nRVIdx-1)-
  2658.                                            m_pRoads[nRd]->VertXDir(nNumVerts-nVert)*fMySide;
  2659.                 }
  2660.             }
  2661.             nCRd=nRRd;
  2662.             nCVIdx=nRVIdx;
  2663.             m_bFinished=false;
  2664.         }
  2665.         else if(!m_bDir[nLRd] && 
  2666.                 (nLRd<nCRd || nLVIdx<m_pRoads[nLRd]->NumVerts()-1))    // road goes to the left
  2667.         {
  2668.             if(m_pRoads[nLRd]->Divider()==1 && fSideDist>0.f)    // Do this just for jersey barriers.
  2669.             {
  2670.                 nNumVerts=m_pRoads[nLRd]->NumVerts();
  2671.                 Vector3 Dir=m_pRoads[nLRd]->CenterVertice(nNumVerts-nLVIdx-1)-
  2672.                             m_pRoads[nLRd]->RBoundary(nNumVerts-nLVIdx-1);
  2673.                 Dir.Normalize();
  2674.                 m_Route[nRouteIdx].Vtx=m_pRoads[nLRd]->CenterVertice(nNumVerts-nLVIdx-1)+Dir*(m_fRSideDist+1.5f);
  2675.             }
  2676.             else
  2677.             {
  2678.                 int nRd=m_Route[nRouteIdx-1].m_nSRd;
  2679.                 int nVert=m_Route[nRouteIdx-1].m_nSVIdx;
  2680.                 if(nVert<1)
  2681.                     nVert=1;
  2682.                 if(m_bDir[nRd])
  2683.                 {
  2684.                     p=m_Route[nRouteIdx-1].Vtx-m_pRoads[nRd]->RBoundary(nVert);
  2685.                     float fMySide=aiDot(p,m_pRoads[nRd]->VertXDir(nVert));
  2686.                     m_Route[nRouteIdx].Vtx=m_pRoads[nLRd]->LBoundary(m_pRoads[nLRd]->NumVerts()-nLVIdx-1)+
  2687.                                            m_pRoads[nRd]->VertXDir(nVert)*fMySide;
  2688.                 }
  2689.                 else
  2690.                 {
  2691.                     nNumVerts=m_pRoads[nRd]->NumVerts();
  2692.                     p=m_Route[nRouteIdx-1].Vtx-m_pRoads[nRd]->LBoundary(nNumVerts-nVert-1);
  2693.                     float fMySide=aiDot(p,-m_pRoads[nRd]->VertXDir(nNumVerts-nVert));
  2694.                     m_Route[nRouteIdx].Vtx=m_pRoads[nLRd]->LBoundary(m_pRoads[nLRd]->NumVerts()-nLVIdx-1)-
  2695.                                            m_pRoads[nRd]->VertXDir(nNumVerts-nVert)*fMySide;
  2696.                 }
  2697.             }
  2698.             nCRd=nLRd;
  2699.             nCVIdx=nLVIdx;
  2700.             m_bFinished=false;
  2701.         }
  2702.  
  2703.         else if(m_bDir[nRRd] && 
  2704.            (nRRd<nCRd || nRVIdx<m_pRoads[nRRd]->NumVerts()-1))            // road goes to the right
  2705.         {
  2706.             if(m_pRoads[nRRd]->Divider()==1 && fSideDist>=0.f)    // Do this just for jersey barriers.
  2707.             {
  2708.                 Vector3 Dir=m_pRoads[nRRd]->CenterVertice(nRVIdx)-
  2709.                             m_pRoads[nRRd]->RBoundary(nRVIdx);
  2710.                 Dir.Normalize();
  2711.                 m_Route[nRouteIdx].Vtx=m_pRoads[nRRd]->CenterVertice(nRVIdx)+Dir*(m_fRSideDist+1.5f);
  2712.             }
  2713.             else
  2714.             {
  2715.                 float fMySide;
  2716.                 int nRd=m_Route[nRouteIdx-1].m_nSRd;
  2717.                 int nVert=m_Route[nRouteIdx-1].m_nSVIdx;
  2718.                 if(m_bDir[nRd])
  2719.                 {
  2720.                     p=m_Route[nRouteIdx-1].Vtx-m_pRoads[nRd]->RBoundary(nVert);
  2721.                     fMySide=aiDot(p,m_pRoads[nRd]->VertXDir(nVert));
  2722.                 }
  2723.                 else
  2724.                 {
  2725.                     p=m_Route[nRouteIdx-1].Vtx-m_pRoads[nRd]->LBoundary(m_pRoads[nRd]->NumVerts()-nVert-1);
  2726.                     fMySide=aiDot(p,-m_pRoads[nRd]->VertXDir(m_pRoads[nRd]->NumVerts()-nVert-1));
  2727.                 }
  2728.                 if(fMySide<m_fRSideDist)
  2729.                     fMySide=m_fRSideDist;
  2730.  
  2731.                 m_Route[nRouteIdx].Vtx=m_pRoads[nRRd]->RBoundary(nRVIdx)+
  2732.                                        m_pRoads[nRRd]->VertXDir(nRVIdx)*fMySide;
  2733.             }
  2734.             nCRd=nRRd;
  2735.             nCVIdx=nRVIdx;
  2736.             m_bFinished=false;
  2737.         }
  2738.         else if(m_bDir[nLRd] && 
  2739.                 (nLRd<nCRd || nLVIdx<m_pRoads[nLRd]->NumVerts()-1))    // road goes to the left
  2740.         {
  2741.             if(m_pRoads[nLRd]->Divider()==1 && fSideDist>0.f)    // Do this just for jersey barriers.
  2742.                 m_Route[nRouteIdx].Vtx=m_pRoads[nLRd]->CenterVertice(nLVIdx)-
  2743.                                        m_pRoads[nLRd]->VertXDir(nLVIdx)*(m_fLSideDist+1.5f);
  2744.             else
  2745.             {
  2746.                 float fMySide;
  2747.                 int nRd=m_Route[nRouteIdx-1].m_nSRd;
  2748.                 int nVert=m_Route[nRouteIdx-1].m_nSVIdx;
  2749.                 if(m_bDir[nRd])
  2750.                 {
  2751.                     p=m_Route[nRouteIdx-1].Vtx-m_pRoads[nRd]->LBoundary(nVert);
  2752.                     fMySide=aiDot(p,-m_pRoads[nRd]->VertXDir(nVert));
  2753.                 }
  2754.                 else
  2755.                 {
  2756.                     p=m_Route[nRouteIdx-1].Vtx-m_pRoads[nRd]->RBoundary(m_pRoads[nRd]->NumVerts()-nVert-1);
  2757.                     fMySide=aiDot(p,m_pRoads[nRd]->VertXDir(m_pRoads[nRd]->NumVerts()-nVert-1));
  2758.                 }
  2759.                 if(fMySide<m_fLSideDist)
  2760.                     fMySide=m_fLSideDist;
  2761.  
  2762.                 m_Route[nRouteIdx].Vtx=m_pRoads[nLRd]->LBoundary(nLVIdx)-
  2763.                                        m_pRoads[nLRd]->VertXDir(nLVIdx)*fMySide;
  2764.             }
  2765.             nCRd=nLRd;
  2766.             nCVIdx=nLVIdx;
  2767.             m_bFinished=false;
  2768.         }
  2769.     }
  2770.  
  2771.     m_Route[nRouteIdx].m_nType=kRoadTarget;
  2772.     m_Route[nRouteIdx].m_nSVIdx=nCVIdx;
  2773.     m_Route[nRouteIdx].m_nSRd=nCRd;
  2774.     m_Route[nRouteIdx].m_nTurnId=nCRd;
  2775.     m_Route[nRouteIdx].Vtx.y+=1.f;
  2776.  
  2777.     m_Route[nRouteIdx].m_fCumDist=m_Route[nRouteIdx-1].m_fCumDist+
  2778.                                   aiDist(m_Route[nRouteIdx-1].Vtx,m_Route[nRouteIdx].Vtx);
  2779.     if(nRouteIdx==1)
  2780.     {
  2781.         Vector3 TargetDir=m_Route[nRouteIdx].Vtx-m_Car.GetMatrix().Pos;
  2782.         m_Route[nRouteIdx].m_fCumAngle=(-m_Car.GetMatrix().ZDir).Angle(TargetDir);
  2783.     }
  2784.     else
  2785.     {
  2786.         Vector3 TargetDir=m_Route[nRouteIdx].Vtx-m_Route[nRouteIdx-1].Vtx;
  2787.         Vector3 PTargetDir=m_Route[nRouteIdx-1].Vtx-m_Route[nRouteIdx-2].Vtx;
  2788.         m_Route[nRouteIdx].m_fCumAngle=m_Route[nRouteIdx-1].m_fCumAngle+
  2789.                                        PTargetDir.Angle(TargetDir);
  2790.     }
  2791. }
  2792.  
  2793. aiObstacle* aiNavigation::IsTargetBlocked(Vector3& Pos,Vector3& Target,int nSVIdx,int nSRIdx,
  2794.                                                int nTVIdx,int nTRIdx,float fMaxDist,int *pnType)
  2795. {
  2796.     if(m_pRoads[nTRIdx] && nTVIdx>m_pRoads[nTRIdx]->NumVerts())
  2797.     {
  2798.         nTRIdx++;
  2799.         nTVIdx=1;
  2800.     }
  2801.  
  2802.     // Check the ambient vehicles
  2803.     aiObstacle *pObstacle, *pOrigObstacle, *pTheObstacle=0;
  2804.     int nStartVert=nSVIdx;
  2805.     float fMinDist=99999.f;
  2806.     for(int ctr=nSRIdx;ctr<nTRIdx+1;ctr++)
  2807.     {
  2808.         if(m_pRoads[ctr])
  2809.         {
  2810.             for(int ctr1=nStartVert;ctr1<m_pRoads[ctr]->NumVerts();ctr1++)
  2811.             {
  2812.                 // Check the vehicles on the right side
  2813.                 if(m_bDir[ctr]==1)
  2814.                 {
  2815.                     if(ctr1==0)
  2816.                         pObstacle=m_pRoads[ctr]->Departure()->Vehicles();
  2817.                     else
  2818.                         pObstacle=m_pRoads[ctr]->Vehicles(ctr1,1);
  2819.                 }
  2820.                 else
  2821.                 {
  2822.                     if(ctr1==0)
  2823.                         pObstacle=m_pRoads[ctr]->Destination()->Vehicles();
  2824.                     else
  2825.                         pObstacle=m_pRoads[ctr]->Vehicles(m_pRoads[ctr]->NumVerts()-ctr1,1);
  2826.                 }
  2827.  
  2828.                 pOrigObstacle=pObstacle;
  2829.                 while(pObstacle)
  2830.                 {
  2831.                     float fObsDist=pObstacle->IsBlockingTarget(Pos,Target,fMaxDist,VehicleWidth());
  2832.                     if(fObsDist>-1.f)
  2833.                     {
  2834.                         if(fObsDist<fMinDist)
  2835.                         {
  2836.                             fMinDist=fObsDist;
  2837.                             pTheObstacle=pObstacle;
  2838.                             *pnType=kAmbObstacle;
  2839.                         }
  2840.                     }  
  2841.                     pObstacle=pObstacle->NextObstacle();
  2842.                     if(pOrigObstacle==pObstacle)
  2843.                         break;
  2844.                 }
  2845.  
  2846.                 if(ctr1==0)    // The vehicle is in the interseciton
  2847.                 {
  2848.                 }
  2849.                 else
  2850.                 {
  2851.                     // Check the left side vehicles
  2852.                     if(m_bDir[ctr]==1)
  2853.                         pObstacle=m_pRoads[ctr]->Vehicles(m_pRoads[ctr]->NumVerts()-ctr1,-1);
  2854.                     else
  2855.                         pObstacle=m_pRoads[ctr]->Vehicles(ctr1,-1);
  2856.                     pOrigObstacle=pObstacle;
  2857.                     while(pObstacle)
  2858.                     {
  2859.                         float fObsDist=pObstacle->IsBlockingTarget(Pos,Target,fMaxDist,VehicleWidth());
  2860.                         if(fObsDist>-1.f)
  2861.                         {
  2862.                             if(fObsDist<fMinDist)
  2863.                             {
  2864.                                 fMinDist=fObsDist;
  2865.                                 pTheObstacle=pObstacle;
  2866.                                 *pnType=kAmbObstacle;
  2867.                             }
  2868.                         }
  2869.                         pObstacle=pObstacle->NextObstacle();
  2870.                         if(pOrigObstacle==pObstacle)
  2871.                             break;
  2872.                     }
  2873.                 }
  2874.  
  2875.                 // Test for finished case.
  2876.                 if((ctr1==nTVIdx && ctr==nTRIdx) || pTheObstacle)
  2877.                 {
  2878.  
  2879.                     ctr=nTRIdx;
  2880.                     ctr1=nTVIdx;
  2881.                     break;
  2882.                 }
  2883.             }
  2884.             nStartVert=0;
  2885.         }
  2886.     }
  2887.  
  2888.     return pTheObstacle;
  2889. }
  2890.  
  2891.